#include <stdio.h>      // vsprintf()
#include <string.h>     // strcpy()
#include <stdarg.h>     // va_start()
#include <stdlib.h>     // exit()
#include <signal.h>     // signal(), SIGINT
#include <float.h>      // DBL_MAX
#include <sys/types.h>  // pid_t
#include <unistd.h>     // fork()
#include <sys/wait.h>   // waitpid(), WNOHANG, WIFEXITED
#include <ctype.h>      // isspace()
#include "config.h"
#include <libintl.h>
#include <locale.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <time.h>
#include <sys/time.h>
#include <sys/reboot.h>
#include "motion/usrmotintf.h"
#include "errcode.h"
#include "log/zuclog.h"
#include "rtapi/rtapi.h"
#include "config.h"
#include "log/datacollector.hh"
#include "safety/io/safeio.h"
#include "rtbus/rtbus.h"
#include "rtdev/rtdev.h"
#include "rtdev/safetyboard/scbitf.h"
#include "log/zlog.hh"
#include "hwdrv/hwdrv.h"

#if 0
// Enable this to niftily trap floating point exceptions for debugging
#include <fpu_control.h>
fpu_control_t __fpu_control = _FPU_IEEE & ~(_FPU_MASK_IM | _FPU_MASK_ZM | _FPU_MASK_OM);
#endif

#include "rcs.hh"           // NML classes, nmlErrorFormat()
#include "nml_intf/zuc.hh"  // ZUC NML
#include "nml_intf/zuc_nml.hh"
#include "nml_intf/canon.hh"  // CANON_TOOL_TABLE stuff
// #include "inifile.hh"          // INIFILE
#include "nml_intf/interpl.hh"          // NML_INTERP_LIST, interp_list
#include "nml_intf/zucglb.h"            // ZUC_INIFILE,NMLFILE, ZUC_TASK_CYCLE_TIME
#include "nml_intf/interp_return.hh"    // public interpreter return values
#include "rs274ngc/interp_internal.hh"  // interpreter private definitions
#include "rcs_print.hh"
#include "timer.hh"
#include "nml_oi.hh"
#include "task.hh"  // zucTaskCommand etc
//#include "taskclass.hh"
#include "motion/motion.h"  // ZUCMOT_ORIENT_*
#include "ini/initraj.hh"
#include <mutex>
#include "rs274ngc/rs274ngc_interp.hh"
#include "taskjoint.hh"
#include "modbus/modbusSlaveIo.h"
#include "log/zuclog.h"
#include "jkutil/iniparser.h"
#include "motion/rtcmds.h"
#include "axis/axis_cmds.h"
#include "funcio/funcio.h"
#include "motion_al.h"
#include "taskintf_config.h"
#include "taskintf.h"
#include "rcstat.h"

extern double servoj_cutoff_freq;

extern std::string get_md5sum(const std::string& filename);

static zucmot_config_t zucmotConfig;
static ModbusSlaveIo _mbio;

/* time after which the user interface is declared dead
 * because it would'nt read any more messages
 */
#define DEFAULT_ZUC_UI_TIMEOUT 5.0

// NML channels
static RCS_CMD_CHANNEL* zucCommandBuffer = 0;
static RCS_STAT_CHANNEL* zucStatusBuffer = 0;
static NML* zucErrorBuffer = 0;

// NML command channel data pointer
static RCS_CMD_MSG* zucCommand = 0;

// global ZUC status
ZUC_STAT* zucStatus = 0;

// timer stuff
static RCS_TIMER* timer = 0;

// flag signifying that ini file [TASK] CYCLE_TIME is <= 0.0, so
// we should not delay at all between cycles. This means also that
// the ZUC_TASK_CYCLE_TIME global will be set to the measured cycle
// time each cycle, in case other code references this.
static int zucTaskNoDelay = 0;
// flag signifying that on the next loop, there should be no delay.
// this is set when transferring trajectory data from userspace to kernel
// space, annd reset otherwise.
static int zucTaskEager = 0;

static int no_force_homing = 0;  // forces the user to home first before allowing MDI and Program run
//can be overriden by [TRAJ]NO_FORCE_HOMING=1

static double ZUC_TASK_CYCLE_TIME_ORIG = 0.0;

// delay counter
static double taskExecDelayTimeout = 0.0;

// static int sj_socket = 0;

// zucTaskIssueCommand issues command immediately
static int zucTaskIssueCommand(NMLmsg* cmd);

static int zucTaskSetGrpcErrCode(NMLmsg* cmd);
// pending command to be sent out by zucTaskExecute()
NMLmsg* zucTaskCommand = 0;

// signal handling code to stop main loop
int done;
static int zuctask_shutdown(void);
extern void backtrace(int signo);
extern void backtraces_new(int signo);
int _task = 1;  // control preview behaviour when remapping
static int threadRunModeWhenMainThreadPause = 0;
extern void setup_signal_handlers();  // backtrace, gdb-in-new-window supportx
std::mutex read_ahead_locker;

int all_homed(void)
{
    // for (int i = 0; i < zucStatus->motion.traj.joints; i++)
    // {
    //     if (!zucStatus->motion.joint[i].homed)  // XXX
    //         return 0;
    // }
    return 1;
}

void zuctask_quit(int sig)
{
    // set main's done flag
    done = 1;
    // restore signal handler
    // signal(sig, zuctask_quit);
}

/* make sure at least space bytes are available on

 * error channel; wait a bit to drain if needed
 */
int zucErrorBufferOKtoWrite(int space, const char* caller)
{
    // check channel for validity
    if (zucErrorBuffer == NULL)
        return -1;
    if (!zucErrorBuffer->valid())
        return -1;

    double send_errorchan_timout = etime() + DEFAULT_ZUC_UI_TIMEOUT;

    while (etime() < send_errorchan_timout)
    {
        if (zucErrorBuffer->get_space_available() < space)
        {
            // esleep(0.01);
            // continue;
            return -1;
        }
        else
        {
            break;
        }
    }
    if (etime() >= send_errorchan_timout)
    {
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print("timeout waiting for error channel to drain, caller=`%s' request=%d\n", caller, space);
        }
        return -1;
    }

    return 0;
}

// implementation of ZUC error logger
int zucOperatorError(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_ERROR error_msg;
    va_list ap;

    if (NULL == fmt)
    {
        return -1;
    }
    if (0 == *fmt)
    {
        return -1;
    }
    // prepend error code, leave off 0 ad-hoc code
    error_msg.errcode = id;
    error_msg.error[0] = 0;
    if (0 != id)
    {
        snprintf(error_msg.error, sizeof(error_msg.error), "[%d] ", id);
    }
    // append error string
    va_start(ap, fmt);
    vsnprintf(&error_msg.error[strlen(error_msg.error)], sizeof(error_msg.error) - strlen(error_msg.error), fmt, ap);
    va_end(ap);

    // force a NULL at the end for safety
    error_msg.error[LINELEN - 1] = 0;

    // write to zuc log file
    zucLogError(error_msg.errcode, error_msg.error);

    if (zucErrorBufferOKtoWrite(sizeof(error_msg) * 2, "zucOperatorError"))
        return -1;

    return zucErrorBuffer->write(error_msg);
}

int zucOperatorWarning(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_ERROR error_msg;
    va_list ap;

    if (NULL == fmt)
    {
        return -1;
    }
    if (0 == *fmt)
    {
        return -1;
    }
    // prepend error code, leave off 0 ad-hoc code
    error_msg.errcode = id;
    error_msg.error[0] = 0;
    if (0 != id)
    {
        snprintf(error_msg.error, sizeof(error_msg.error), "[%d] ", id);
    }
    // append error string
    va_start(ap, fmt);
    vsnprintf(&error_msg.error[strlen(error_msg.error)], sizeof(error_msg.error) - strlen(error_msg.error), fmt, ap);
    va_end(ap);

    // force a NULL at the end for safety
    error_msg.error[LINELEN - 1] = 0;

    // write to zuc log file
    zucLogWarning(error_msg.errcode, error_msg.error);
    if (zucErrorBufferOKtoWrite(sizeof(error_msg) * 2, "zucOperatorError"))
        return -1;

    return zucErrorBuffer->write(error_msg);
}

int zucOperatorText(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_TEXT text_msg;
    va_list ap;

    text_msg.id = id;
    // write args to NML message (ignore int text code)
    va_start(ap, fmt);
    vsnprintf(text_msg.text, sizeof(text_msg.text), fmt, ap);
    va_end(ap);
    // force a NULL at the end for safety
    text_msg.text[LINELEN - 1] = 0;

    // write to zuc log file
    zucLogInfo(text_msg.id, text_msg.text);

    if (zucErrorBufferOKtoWrite(sizeof(text_msg) * 2, "zucOperatorText"))
        return -1;

    return zucErrorBuffer->write(text_msg);
}

int zucOperatorDisplay(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_DISPLAY display_msg;
    va_list ap;

    if (zucErrorBufferOKtoWrite(sizeof(display_msg) * 2, "zucOperatorDisplay"))
        return -1;

    // write args to NML message (ignore int display code)
    va_start(ap, fmt);
    vsnprintf(display_msg.display, sizeof(display_msg.display), fmt, ap);
    va_end(ap);

    display_msg.id = id;
    // force a NULL at the end for safety
    display_msg.display[LINELEN - 1] = 0;

    // write it
    return zucErrorBuffer->write(display_msg);
}

/*
 handling of ZUC_SYSTEM_CMD
 */

/* convert string to arg/argv set */

static int argvize(const char* src, char* dst, char* argv[], int len)
{
    char* bufptr;
    int argvix;
    char inquote;
    char looking;

    strncpy(dst, src, len);
    dst[len - 1] = 0;
    bufptr = dst;
    inquote = 0;
    argvix = 0;
    looking = 1;

    while (0 != *bufptr)
    {
        if (*bufptr == '"')
        {
            *bufptr = 0;
            if (inquote)
            {
                inquote = 0;
                looking = 1;
            }
            else
            {
                inquote = 1;
            }
        }
        else if (isspace(*bufptr) && !inquote)
        {
            looking = 1;
            *bufptr = 0;
        }
        else if (looking)
        {
            looking = 0;
            argv[argvix] = bufptr;
            argvix++;
        }
        bufptr++;
    }

    argv[argvix] = 0;  // null-terminate the argv list

    return argvix;
}

static pid_t zucSystzucmdPid = 0;

int zucSystzucmd(char* s)
{
    char buffer[ZUC_SYSTEM_CMD_LEN];
    char* argv[ZUC_SYSTEM_CMD_LEN / 2 + 1] = {0};

    if (0 != zucSystzucmdPid)
    {
        // something's already running, and we can only handle one
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print("zucSystzucmd: abandoning process %d, running ``%s''\n", zucSystzucmdPid, s);
        }
    }

    zucSystzucmdPid = fork();

    if (-1 == zucSystzucmdPid)
    {
        // we're still the parent, with no child created
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print("system command ``%s'' can't be executed\n", s);
        }
        return -1;
    }

    if (0 == zucSystzucmdPid && argv[0])
    {
        // we're the child
        // convert string to argc/argv
        argvize(s, buffer, argv, ZUC_SYSTEM_CMD_LEN);
        execvp(argv[0], argv);
        // if we get here, we didn't exec
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print("zucSystzucmd: can't execute ``%s''\n", s);
        }
        exit(-1);
    }
    // else we're the parent
    return 0;
}

// shorthand typecasting ptrs
static ZUC_JOG_CONT* jog_cont_msg;
static ZUC_JOG_STOP* jog_stop_msg;
static ZUC_JOG_INCR* jog_incr_msg;
static ZUC_JOG_ABS* jog_abs_msg;
static ZUC_JOG_MOVJ* movj_incr_msg;
static ZUC_JOG_MOVL* movl_incr_msg;
static ZUC_JOG_MOVC* movc_msg;
static ZUC_JOINT_SET_MAX_POSITION_LIMIT* set_max_limit_msg;
static ZUC_JOINT_SET_MIN_POSITION_LIMIT* set_min_limit_msg;

static ZUC_TRAJ_SET_SCALE* zucTrajSetScaleMsg;
static ZUC_TRAJ_SET_RAPID_SCALE* zucTrajSetRapidScaleMsg;
static ZUC_TRAJ_LINEAR_MOVE* zucTrajLinearMoveMsg;
static ZUC_TRAJ_CIRCULAR_MOVE* zucTrajCircularMoveMsg;
static ZUC_TRAJ_JOINT_MOVE* zucTrajJointMoveMsg;
static ZUC_TRAJ_DELAY* zucTrajDelayMsg;
static ZUC_TRAJ_SET_ROBOT_USER_FRAME* zucTrajSetRobotUserFrmsetMsg;
static ZUC_TRAJ_SET_PAYLOAD* zucTrajSetPayloadMsg;
static ZUC_TRAJ_CALIB_DHPARAM* zucTrajCalibDHParamMsg;

// These classes are commented out because the compiler
// complains that they are "defined but not used".
//static ZUC_MOTION_SET_AOUT *zucMotionSetAoutMsg;
//static ZUC_MOTION_SET_DOUT *zucMotionSetDoutMsg;

static ZUC_TASK_SET_MODE* mode_msg;
static ZUC_TASK_SET_OPERATION_MODE* opmode_msg;
static ZUC_TASK_SET_AUTOWORK* autowork_msg;
static ZUC_TASK_SET_STATE* state_msg;
static ZUC_TASK_PLAN_RUN* run_msg;
static ZUC_TASK_PLAN_EXECUTE* execute_msg;
static ZUC_TASK_PLAN_OPEN* open_msg;
static ZUC_TASK_PLAN_SET_OPTIONAL_STOP* os_msg;
static ZUC_TASK_PLAN_SET_BLOCK_DELETE* bd_msg;
static ZUC_TASK_CALC_TCP* calc_tcp_msg;
static ZUC_TASK_CALC_USER* calc_user_msg;
static ZUC_TASK_SET_USER_DEFINED_VARIABLE* user_defined_var;
static ZUC_AUX_INPUT_WAIT* zucAuxInputWaitMsg;
static ZUC_SET_MUTI_TOOL_OFFSET* set_muti_tool_offset_msg;
static ZUC_SET_MUTI_USER_OFFSET* set_muti_user_offset_msg;
static ZUC_SET_TOOL_ID* set_tool_id_msg;
static ZUC_TRAJ_MOVC* zucTrajMovcMsg;
static ZUC_TRAJ_MOVS* zucTrajMovsMsg;
static ZUC_TASK_CALC_SAFEATT* calc_safeatt_msg;

static ZUC_SET_USER_ID* set_user_id_msg;
static ZUC_CONVEYOR_ENABLE* conveyor_enable_msg;
static ZUC_CONVEYOR_DISABLE* conveyor_disable_msg;
static ZUC_TOPPRA_FINISH_LEFT_TC* toppra_finish_left_tc_msg;
static ZUC_TOPPRA_FIRST_COMMAND* toppra_first_command_msg;

static int zucAuxInputWaitDIType = -1;
static int zucAuxInputWaitIndex = -1;
static int zucAuxInputWaitType = 0;

// commands we compose here
static ZUC_TASK_PLAN_RUN taskPlanRunCmd;  // 16-Aug-1999 FMP
static ZUC_TASK_PLAN_INIT taskPlanInitCmd;
static ZUC_TASK_PLAN_SYNCH taskPlanSynchCmd;

static ZUC_ROBOT_SAFETY* set_robot_safety;
static ZUC_JOINT_SAFETY* set_joint_safety;
static ZUC_STEP_STATUS* step_status;
static ZUC_REDUCE_SAFETY* set_reduce_safety;

static int interpResumeState = ZUC_TASK_INTERP_IDLE;
static int programStartLine = 0;  // which line to run program from
// how long the interp list can be

int stepping = 0;
int steppingLine = 0;
int steppingWait = 0;
static int steppedLine = 0;

// Variables to handle MDI call interrupts
// Depth of call level before interrupted MDI call
static int mdi_execute_level = -1;
// Schedule execute(0) command
static int mdi_execute_next = 0;
// Wait after interrupted command
static int mdi_execute_wait = 0;
// Side queue to store MDI commands
static NML_INTERP_LIST mdi_execute_queue;

// MDI input queue
static NML_INTERP_LIST mdi_input_queue;
#define MAX_MDI_QUEUE 10
static int max_mdi_queued_commands = MAX_MDI_QUEUE;

static void checkExtioStat()
{
    static int last_extio_errCode = 0;
    int extio_errCode = zucStatus->io.extio.extIOStat.errCode;
    if (zucStatus->io.extio.extIOStat.status == 2)
    {  // fault
        if (extio_errCode != 0 && last_extio_errCode != extio_errCode)
        {
            switch (zucStatus->io.extio.extIOStat.errCode)
            {
            case 0x1102:
                zucOperatorError(EXTIO_ERR_INIT_RTU_CONNECTION, "failed to create rtu connection during initialization.");
                break;
            case 0x1202:
                zucOperatorError(EXTIO_ERR_INIT_TCP_CONNECTION, "failed to create tcp connection during initialization.");
                break;
            case 0x1304:
                zucOperatorError(EXTIO_ERR_INIT_UNKNOWN, "unknown error during initalization.");
                break;
            case 0x1101:
                zucOperatorError(EXTIO_ERR_INIT_RTU_INCONSISTENT, "rtu inconsistent during initialization.");
                break;
            case 0x1201:
                zucOperatorError(EXTIO_ERR_INIT_TCP_FORMAT, "tcp format error during initialization.");
                break;
            case 0x2103:
                zucOperatorError(EXTIO_ERR_RUNTIME_RTU_OFFLINE, "rtu node offline during runtime.");
                break;
            case 0x2203:
                zucOperatorError(EXTIO_ERR_RUNTIME_TCP_OFFLINE, "tcp node offline during runtime.");
                break;
            default:
                zucOperatorError(UNKNOWN_ERROR, "unknown error 0x%x in extio module.", zucStatus->io.extio.extIOStat.errCode);
                break;
            }
            zucTaskAbort();
            zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
        }
        zucExtIOSetMode(0);
    }
    last_extio_errCode = extio_errCode;
}

static void checkSensorMgrStat()
{
    static int last_sensor_errCode = 0;
    int sensor_errCode = zucStatus->motion.sensor.sensorStatus.errCode;
    if (zucStatus->motion.sensor.sensorStatus.status == 2)
    {  // fault
        // rcs_print("torque sensor error detected, sensor_errCode=0x%x\n", sensor_errCode);
        if (sensor_errCode != 0 && last_sensor_errCode != sensor_errCode)
        {
            switch (zucStatus->motion.sensor.sensorStatus.errCode)
            {
            case 0x0001:
                zucOperatorError(TORQSENSOR_ERR_FAILED_INIT_CONN, "failed to initialize the connection with torque sensor");
                break;
            case 0x1001:
                zucOperatorError(TORQSENSOR_ERR_CANNOT_RECV_DATA, "cannot receive data from torque sensor");
                break;
            case 0x1002:
                zucOperatorError(TORQSENSOR_ERR_RECV_WRONG_DATA, "wrong data is received from torque sensor");
                break;
            default:
                zucOperatorError(UNKNOWN_ERROR, "unknown error in sensor module.");
                break;
            }
            // 如果开启了力控，需要停止程序; 否则不终止程序
            if (zucStatus->motion.compliantType == 1 || zucStatus->motion.compliantType == 2)
            {
                zucTaskAbort();
                zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
            }
        }
        // zucTorqSensorSetMode(robot_id, 0);
    }

    last_sensor_errCode = sensor_errCode;
}

/*
 checkInterpList(NML_INTERP_LIST *il, ZUC_STAT *stat) takes a pointer
 to an interpreter list and a pointer to the ZUC status, pops each NML
 message off the list, and checks it against limits, resource availability,
 etc. in the status.

 It returns 0 if all messages check out, -1 if any of them fail. If one
 fails, the rest of the list is not checked.
 */
static int checkInterpList(NML_INTERP_LIST* il, ZUC_STAT* stat)
{
    NMLmsg* cmd = 0;
    // let's create some shortcuts to casts at compile time
#define operator_error_msg ((ZUC_OPERATOR_ERROR*)cmd)
#define linear_move ((ZUC_TRAJ_LINEAR_MOVE*)cmd)
#define circular_move ((ZUC_TRAJ_CIRCULAR_MOVE*)cmd)

    while (il->len() > 0)
    {
        cmd = il->get();

        switch (cmd->type)
        {
        case ZUC_OPERATOR_ERROR_TYPE:
            zucOperatorError(operator_error_msg->errcode, "%s", operator_error_msg->error);
            break;

            //FIXME: there was limit checking tests below, see if they were needed
        case ZUC_TRAJ_LINEAR_MOVE_TYPE:
            break;

        case ZUC_TOPPRA_FINISH_LEFT_TC_TYPE:
            break;

        case ZUC_TOPPRA_FIRST_COMMAND_TYPE:
            break;

        case ZUC_TRAJ_CIRCULAR_MOVE_TYPE:
            break;

        case ZUC_TRAJ_JOINT_MOVE_TYPE:
            break;
        case ZUC_TRAJ_MOVC_TYPE:
            break;
        default:
            break;
        }
    }

    return 0;

    // get rid of the compile-time cast shortcuts
#undef circular_move_msg
#undef linear_move_msg
#undef operator_error_msg
}

extern int zucTaskMopup();

void readahead_reading(void)
{
    std::lock_guard<std::mutex> g(read_ahead_locker);
    int readRetval;
    int execRetval;
    if (interp_list.len() <= zuc_task_interp_max_len)
    {
        int count = 0;
    interpret_again:
        if (zucTaskPlanIsWait())
        {
            // delay reading of next line until all is done
            if (interp_list.len() == 0 && zucTaskCommand == 0 && zucStatus->task.execState == ZUC_TASK_EXEC_DONE)
            {
                zucTaskPlanClearWait();
            }
        }
        else
        {
            // rcs_print(">>>>>>>>>read ahead reading>>>\n");
            readRetval = zucTaskPlanRead();
            /*! \todo MGS FIXME
			 This if() actually evaluates to if (readRetval != INTERP_OK)...
			 *** Need to look at all calls to things that return INTERP_xxx values! ***
			 MGS */
            if (readRetval > INTERP_MIN_ERROR)
            {
                zucOperatorDisplay(INTERP_EXECUTION_ERROR, "Program execution error.");
                zucTaskAbort();
            }
            else if (readRetval == INTERP_ENDFILE || readRetval == INTERP_EXIT || readRetval == INTERP_EXECUTE_FINISH)
            {
                /* zucTaskPlanRead retval != INTERP_OK
				 Signal to the rest of the system that that the interp is now in a paused state. */
                zucStatus->task.interpState = ZUC_TASK_INTERP_WAITING;
            }
            else
            {
                // got a good line, record the line number and command
                zucStatus->task.readLine = zucTaskPlanLine();
                zucTaskPlanCommand((char*)&zucStatus->task.command);
                execRetval = zucTaskPlanExecute(0);
                // line number may need update after returns from subprograms in external files
                zucStatus->task.readLine = zucTaskPlanLine();
                if (execRetval > INTERP_MIN_ERROR)
                {
                    zucOperatorDisplay(INTERP_EXECUTION_ERROR, "Program execution error.");
                    zucTaskAbort();
                    zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
                    interp_list.clear();
                    zucAbortCleanup(ZUC_ABORT_INTERPRETER_ERROR, "interpreter error");
                }
                else if (execRetval == -1 || execRetval == INTERP_EXIT)
                {
                    zucStatus->task.interpState = ZUC_TASK_INTERP_WAITING;
                }
                else if (execRetval == INTERP_EXECUTE_FINISH || execRetval == INTERP_WAIT)
                {
                    // INTERP_EXECUTE_FINISH signifies that no more reading should be done until
                    // everything outstanding is completed
                    zucTaskPlanSetWait();
                    // and resynch interp WM
                    zucTaskQueueCommand(&taskPlanSynchCmd);
                }
                else if (execRetval != 0)
                {
                    // end of file
                    zucStatus->task.interpState = ZUC_TASK_INTERP_WAITING;
                    zucStatus->task.motionLine = 0;
                    zucStatus->task.readLine = 0;
                    zucStatus->task.sleepLine = 0;
                }
                else
                {
                    // executed a good line
                }

                // throw the results away if we're supposed to read  through it
                if ((programStartLine < 0 || zucStatus->task.readLine < programStartLine) && zucTaskPlanLevel() == 0)
                {
                    // we're stepping over lines, so check them for limits, etc. and clear then out
                    if (0 != checkInterpList(&interp_list, zucStatus))
                    {
                        // problem with actions, so do same as we did for a bad read from zucTaskPlanRead() above
                        zucStatus->task.interpState = ZUC_TASK_INTERP_WAITING;
                    }
                    // and clear it regardless
                    interp_list.clear();
                }

                if (zucStatus->task.readLine < programStartLine && zucTaskPlanLevel() == 0)
                {
                    //update the position with our current position, as the other positions are only skipped through
                    CANON_UPDATE_END_POINT(zucStatus->motion.traj.actualPosition.tran.x,
                                           zucStatus->motion.traj.actualPosition.tran.y,
                                           zucStatus->motion.traj.actualPosition.tran.z,
                                           zucStatus->motion.traj.actualPosition.a,
                                           zucStatus->motion.traj.actualPosition.b,
                                           zucStatus->motion.traj.actualPosition.c,
                                           zucStatus->motion.traj.actualPosition.u,
                                           zucStatus->motion.traj.actualPosition.v,
                                           zucStatus->motion.traj.actualPosition.w);

                    if ((zucStatus->task.readLine + 1 == programStartLine) && (zucTaskPlanLevel() == 0))
                    {
                        zucTaskPlanSynch();
                        // reset programStartLine so we don't fall into our stepping routines
                        // if we happen to execute lines before the current point later (due to subroutines).
                        programStartLine = 0;
                    }
                }

                if (stepping == 0 && count++ < zuc_task_interp_max_len && zucStatus->task.interpState == ZUC_TASK_INTERP_READING &&
                    interp_list.len() <= zuc_task_interp_max_len * 2 / 3)
                {
                    goto interpret_again;
                }

            }  // else read was OK, so execute
        }      // else not zucTaskPlanIsWait
    }          // if interp len is less than max
}

static void mdi_execute_abort(void)
{
    int queued_mdi_commands;

    // XXX: Reset needed?
    if (mdi_execute_wait || mdi_execute_next)
        zucTaskPlanReset();
    mdi_execute_level = -1;
    mdi_execute_wait = 0;
    mdi_execute_next = 0;

    queued_mdi_commands = mdi_execute_queue.len();
    if (queued_mdi_commands > 0)
    {
        rcs_print("mdi_execute_abort: dropping %d queued MDI commands\n", queued_mdi_commands);
    }
    mdi_execute_queue.clear();
    zucStatus->task.queuedMDIcommands = 0;

    zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
}

static void mdi_execute_hook(void)
{
    if (mdi_execute_wait && zucTaskPlanIsWait())
    {
        // delay reading of next line until all is done
        if (interp_list.len() == 0 && zucTaskCommand == 0 && zucStatus->task.execState == ZUC_TASK_EXEC_DONE)
        {
            zucTaskPlanClearWait();
            mdi_execute_wait = 0;
            mdi_execute_hook();
        }
        return;
    }

    if ((mdi_execute_level < 0) && (mdi_execute_wait == 0) && (mdi_execute_queue.len() > 0) && (interp_list.len() == 0) && (zucTaskCommand == NULL))
    {
        interp_list.append(mdi_execute_queue.get());
        zucStatus->task.queuedMDIcommands = mdi_execute_queue.len();
        return;
    }

    // determine when a MDI command actually finishes normally.
    if (interp_list.len() == 0 && zucTaskCommand == 0 && zucStatus->task.execState == ZUC_TASK_EXEC_DONE &&
        zucStatus->task.interpState != ZUC_TASK_INTERP_IDLE && zucStatus->motion.traj.queue == 0 && !mdi_execute_wait && !mdi_execute_next)
    {
        // finished. Check for dequeuing of queued MDI command is done in zucTaskPlan().
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
            rcs_print("mdi_execute_hook: MDI command '%s' done (remaining: %d)\n", zucStatus->task.command, mdi_input_queue.len());
        zucStatus->task.command[0] = 0;
        zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
    }

    if (!mdi_execute_next)
        return;

    if (interp_list.len() > zuc_task_interp_max_len)
        return;

    mdi_execute_next = 0;

    ZUC_TASK_PLAN_EXECUTE msg;
    msg.command[0] = (char)0xff;

    interp_list.append(msg);
}

void readahead_waiting(void)
{
    // now handle call logic
    // check for subsystems done
    if (interp_list.len() == 0 && zucTaskCommand == 0 && zucStatus->motion.traj.queue == 0)
    // finished
    {
        int was_open = taskplanopen;
        if (was_open)
        {
            printf(">>>zucTaskPlanClose(); 1\n");
            zucTaskPlanClose();
            if (zuc_debug & ZUC_DEBUG_INTERP && was_open)
            {
                rcs_print("zucTaskPlanClose() called at %s:%d\n", __FILE__, __LINE__);
            }
            // then resynch interpreter
            zucTaskQueueCommand(&taskPlanSynchCmd);
        }
        else
        {
            zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
            zucOperatorDisplay(INTERP_EXECUTION_COMPLITED, "Program execution completed.");
        }
        zucStatus->task.readLine = 0;
    }
    else
    {
        // still executing
    }
}

void auxInputWaitMsg(ZUC_AUX_INPUT_WAIT* zucAuxInputWaitMsg)
{
    if (zucAuxInputWaitMsg->timeout == WAIT_MODE_IMMEDIATE)
    {                                       //nothing to do, CANON will get the needed value when asked by the interp
        zucStatus->task.input_timeout = 0;  // no timeout can occur
        zucTaskTimeoutUpdate();             // update to interp module
        zucAuxInputWaitIndex = -1;
        taskExecDelayTimeout = 0.0;
        zucStatus->task.sleepLine = zucAuxInputWaitMsg->line_num;
    }
    else
    {
        zucAuxInputWaitType = zucAuxInputWaitMsg->wait_type;     // remember what we are waiting for
        zucAuxInputWaitDIType = zucAuxInputWaitMsg->input_type;  // remember the input type
        zucAuxInputWaitIndex = zucAuxInputWaitMsg->index;        // remember the input to look at
        zucStatus->task.input_timeout = 2;                       // set timeout flag, gets cleared if input changes before timeout happens
        zucTaskTimeoutUpdate();                                  // update to interp module
        // set the timeout clock to expire at 'now' + delay time
        taskExecDelayTimeout = etime() + zucAuxInputWaitMsg->timeout;
        zucStatus->task.sleepLine = zucAuxInputWaitMsg->line_num;
    }
}

void subThreadAuxInputWaitMsg(ZUC_AUX_INPUT_WAIT* zucAuxInputWaitMsg, ThreadInterpInfo* pThreadInterpInfo)
{
    pThreadInterpInfo->auxInputWaitMsg.waiting = true;
    if (zucAuxInputWaitMsg->timeout == WAIT_MODE_IMMEDIATE)
    {  //nothing to do, CANON will get the needed value when asked by the interp
        pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitIndex = -1;
        pThreadInterpInfo->auxInputWaitMsg.taskExecDelayTimeout = 0.0;
    }
    else
    {
        pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitType = zucAuxInputWaitMsg->wait_type;     // remember what we are waiting for
        pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitDIType = zucAuxInputWaitMsg->input_type;  // remember the input type
        pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitIndex = zucAuxInputWaitMsg->index;        // remember the input to look at
        // set the timeout clock to expire at 'now' + delay time
        pThreadInterpInfo->auxInputWaitMsg.taskExecDelayTimeout = etime() + zucAuxInputWaitMsg->timeout;
    }
}

static bool allow_while_idle_type()
{
    // allow for ZUC_TASK_MODE_AUTO, ZUC_TASK_MODE_MDI
    // expect immediate command
    int type;
    if (multi_thread_flag)
    {
        type = zucCommand->type;
        if (zucCommand == NULL)
            return false;
    }
    else
    {
        RCS_CMD_MSG* zucCommand;
        zucCommand = zucCommandBuffer->get_address();
        type = zucCommand->type;
    }

    switch (type)
    {
    // immediate commands
    case ZUC_JOINT_SET_MAX_POSITION_LIMIT_TYPE:
    case ZUC_JOINT_SET_MIN_POSITION_LIMIT_TYPE:
    case ZUC_JOG_CONT_TYPE:
    case ZUC_JOG_INCR_TYPE:
    case ZUC_JOG_MOVJ_TYPE:
    case ZUC_JOG_MOVL_TYPE:
    case ZUC_JOG_MOVC_TYPE:
    case ZUC_JOG_STOP_TYPE:
    case ZUC_JOG_ABS_TYPE:
    case ZUC_JOG_MULTI_JOINT_TYPE:
    case ZUC_TRAJ_PAUSE_TYPE:
    case ZUC_TRAJ_RESUME_TYPE:
    case ZUC_TRAJ_ABORT_TYPE:
    case ZUC_TRAJ_SET_SCALE_TYPE:
    case ZUC_TRAJ_SET_RAPID_SCALE_TYPE:
    case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE:
    case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
    case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
    case ZUC_TRAJ_SET_PAYLOAD_TYPE:
    case ZUC_TASK_SET_MODE_TYPE:
    case ZUC_TASK_SET_OPERATION_MODE_TYPE:
    case ZUC_TASK_SET_STATE_TYPE:
    case ZUC_MOTION_ABORT_TYPE:
    case ZUC_TASK_ABORT_TYPE:
    case ZUC_TASK_PLAN_OPEN_TYPE:
    case ZUC_TASK_PLAN_CLOSE_TYPE:
    case ZUC_TASK_PLAN_PAUSE_TYPE:
    case ZUC_TASK_PLAN_RESUME_TYPE:
    case ZUC_TASK_PLAN_INIT_TYPE:
    case ZUC_TASK_PLAN_SYNCH_TYPE:
    case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
    case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
    case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
    case ZUC_TASK_CALC_TCP_TYPE:
    case ZUC_TASK_CALC_SAFEATT_TYPE:
    case ZUC_TASK_CALC_USER_TYPE:
    case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
    case ZUC_AUX_INPUT_WAIT_TYPE:
    case ZUC_MOTION_SET_DOUT_TYPE:
    case ZUC_MOTION_SET_AOUT_TYPE:
    case ZUC_MOTION_ADAPTIVE_TYPE:
    case ZUC_MOTION_EXIT_PROTECT_TYPE:
    case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
    case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
    case ZUC_MOTION_REDUCE_MODE_TYPE:
    case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
    case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
    case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
    case ZUC_MOTION_KINE_INVERSE_TYPE:
    case ZUC_MOTION_KINE_FORWARD_TYPE:
    case ZUC_MOTION_SET_HOME_VEL_TYPE:
    case ZUC_TRAJ_RIGID_TAP_TYPE:
    case ZUC_TRAJ_SET_TELEOP_ENABLE_TYPE:
    case ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE_TYPE:
    case ZUC_TRAJ_SET_SERVOJOP_ENABLE_TYPE:
    case ZUC_TRAJ_SET_ADMITOP_ENABLE_TYPE:
    case ZUC_TRAJ_SET_COMPLIANCE_ENABLE_TYPE:
    case ZUC_TORQUE_CONTROL_ENABLE_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_FRAME_TYPE:
    case ZUC_TRAJ_DISABLE_FORCE_CONTROL_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_OPTION_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_CONFIG_TYPE:
    case ZUC_TRAJ_SET_END_FORCE_CONDITION_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_ENABLE_TYPE:
    case ZUC_SET_DEBUG_TYPE:
    case ZUC_SET_SERVO_PARAM_TYPE:
    case ZUC_TASK_SET_AUTOWORK_TYPE:
    case ZUC_SET_ROBOT_NAME_TYPE:
    case ZUC_SET_CABINET_ID_TYPE:
    case ZUC_SET_CUSTOM_POSE_TYPE:
    case ZUC_MOTION_ONE_KEY_OP_TYPE:
    case ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_STARTPOS_TYPE:
    case ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_ENDPOS_TYPE:
    case ZUC_TASK_START_PAYLOAD_IDENTIFY_TRAJ_TYPE:
    case ZUC_GET_PAYLOAD_IDENTIFY_RESULT_TYPE:
    case ZUC_TASK_RESET_PAYLOAD_IDENTIFY_STATUS_TYPE:
    case ZUC_TASK_START_DYNAMICS_IDENTIFY_TYPE:
    case ZUC_TASK_STOP_DYNAMICS_IDENTIFY_TYPE:
    case ZUC_STOP_IDENTIFY_FRICTION_TYPE:
    case ZUC_START_IDENTIFY_FRICTION_TYPE:
    case ZUC_IDENTIFY_FRICTION_END_POS_TYPE:
    case ZUC_IDENTIFY_FRICTION_START_POS_TYPE:
    case ZUC_SERVO_DYNAMICS_PARA_TYPE:
    case ZUC_SERVO_FRICTION_TYPE:
    case ZUC_COLLISION_SENSITIVITY_TYPE:
    case ZUC_BACKDRIVE_ON_TYPE:
    case ZUC_FORCE_LIMIT_VALUE_TYPE:
    case ZUC_COLLISION_DETECTION_ENABLE_TYPE:
    case ZUC_RS485_MESSAGE_TYPE:
    case ZUC_TIO_RS485_SIG_PROG_STAT_TYPE:
    case ZUC_GRAVITY_DIRECTION_TYPE:
    case ZUC_DRAG_CONTROL_ENABLE_TYPE:
    case ZUC_DRAG_CONTROL_FC_COMPENSATE_TYPE:
    case ZUC_DRAG_CONTROL_FV_COMPENSATE_TYPE:
    case ZUC_DRAG_CONTROL_FILTER_FACTOR_TYPE:
    case ZUC_DRAG_CONTROL_DITHER_FACTOR_TYPE:
    case ZUC_IDENTIFY_CYCLE_TIME_TYPE:
    case ZUC_RETURN_TO_INITIAL_POSITION_TYPE:
    case ZUC_MOTION_SET_CAPTURE_TYPE:
    case ZUC_MOTION_CLEAR_CAPTURE_TYPE:
    case ZUC_SET_SERVOJ_PARAMETER_TYPE:
    case ZUC_TORQSENSOR_SETMODE_TYPE:
    case ZUC_TORQUE_SENSOR_SOFT_LIMIT_TYPE:
    case ZUC_ADMITTANCE_DRAG_DEAD_ZONE_TYPE:
    case ZUC_TORQUE_SENSOR_FILTER_TYPE:
    case ZUC_TORQSENSOR_SETBRAND_TYPE:
    case ZUC_TORQSENSOR_SETPAYLOAD_TYPE:
    case ZUC_CONVEYOR_ENABLE_TYPE:
    case ZUC_CONVEYOR_DISABLE_TYPE:
    case ZUC_ROBOT_SAFETY_TYPE:
    case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
    case Robot_7_DOF_INV_INFORM_TYPE:
    case Robot_SET_FULL_DH_FLAG_TYPE:
    case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
    case ZUC_REDUCE_SAFETY_TYPE:
    case ZUC_DRAG_SPEED_LIMIT_TYPE:
    case ZUC_JOINT_SAFETY_TYPE:
    case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE:
    case ZUC_STEP_STATUS_TYPE:
    case ZUC_SET_SAFE_ZONE_TYPE:
    case ZUC_SET_SAFE_ATTITUDE_TYPE:
    case ZUC_SET_PLANNER_JERK_TYPE:
    case ZUC_MBTIO_ADD_SIGNAL_TYPE:
    case ZUC_MBTIO_DEL_SIGNAL_TYPE:
    case ZUC_MBTIO_UPDATE_SIGNAL_TYPE:
    case ZUC_MBTIO_SEND_COMMAND_TYPE:
    case ZUC_SET_COLLISION_OPTION_TYPE:
    case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE:
    case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE:
    case ZUC_SET_GRPC_ERRCODE_TYPE:
    case ZUC_COLLISION_GM_RECORD_ENABLE_TYPE:
    case ZUC_FUNCTION_SAFTY_PARA_TYPE:
    case ZUC_APP_SET_CONNECTSTAT_TYPE:
    case ZUC_SET_SIM_ENABLE_TYPE:
    case ZUC_SET_SIM_ROBOT_TYPE:
    case ZUC_SET_DRAG_ENABLE_TYPE:
    case ZUC_SERVO_HOME_TYPE:
    case ZUC_CLEAR_ERROR_TYPE:
        return 1;
        break;
    }
    return 0;
}

/*
 zucTaskPlan()

 Planner for NC code or manual mode operations
 */
static int zucTaskPlan(void)
{
    NMLTYPE type;
    int retval = 0;
    if (multi_thread_flag && zucCommand == NULL)
    {
        if (zucStatus->task.mode == ZUC_TASK_MODE_AUTO && zucStatus->task.interpState == ZUC_TASK_INTERP_READING)
        {
            readahead_reading();
        }
        return 0;
    }

    // check for new command
    if (zucCommand->serial_number != zucStatus->echo_serial_number || zucCommand->type != zucStatus->command_type)
    {
        // flag it here locally as a new command
        type = zucCommand->type;
#ifdef _DEBUG
        rcs_print("zucTaskPlan: new command %d\n", type);
#endif
    }
    else
    {
        // no new command-- reset local flag
        type = 0;
    }

    switch (type)
    {
    case ZUC_COLLECT_DIAG_DATA_TYPE:
        return zucDiagCollectData(((ZUC_COLLECT_DIAG_DATA*)zucCommand)->isStart);
    case ZUC_SET_GRPC_ERRCODE_TYPE:
        return zucTaskSetGrpcErrCode(zucCommand);
    default:
        break;
    }

    if (zucStatus->motion.three_position_enable_limit == 1 && type == ZUC_TASK_PLAN_STEP_TYPE)
    {
        zucTaskAbort();
        zucAbortCleanup(ZUC_ABORT_TASK_ABORT);
        zucOperatorError(ERROR_THREE_POSITION_ENABLING_LIMIT, _("Can't debug when the three positions enable limits"));
        auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
        if (robot_motion_task.get())
        {
            robot_motion_task->zucTrajSetMode(ZUC_TRAJ_MODE_FREE);
        }
        return 0;
    }

    // handle any new command
    switch (zucStatus->task.state)
    {
    case ZUC_TASK_STATE_OFF:
    case ZUC_TASK_STATE_POWERED_OFF:
    case ZUC_TASK_STATE_POWERED_ON:
        // now switch on the mode
        switch (zucStatus->task.mode)
        {
        case ZUC_TASK_MODE_MANUAL:
        case ZUC_TASK_MODE_AUTO:
        case ZUC_TASK_MODE_MDI:
        case ZUC_TASK_MODE_DRAG:
            // now switch on the command
            switch (type)
            {
            case 0:
            case ZUC_NULL_TYPE:
                break;

            // immediate commands
            case ZUC_APP_SET_CONNECTSTAT_TYPE:
            case ZUC_JOINT_SET_MAX_POSITION_LIMIT_TYPE:
            case ZUC_JOINT_SET_MIN_POSITION_LIMIT_TYPE:
            case ZUC_COLLISION_GM_RECORD_ENABLE_TYPE:
            case ZUC_TRAJ_SET_SCALE_TYPE:
            case ZUC_TRAJ_SET_RAPID_SCALE_TYPE:
            case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE:
            case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
            case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
            case ZUC_TRAJ_SET_ACCELERATION_TYPE:
            case ZUC_TASK_INIT_TYPE:
            case ZUC_TASK_SET_MODE_TYPE:
            case ZUC_TASK_SET_OPERATION_MODE_TYPE:
            case ZUC_AXIS_CMD_POWER_TYPE:
            case ZUC_AXIS_CMD_RESET_TYPE:
            case ZUC_TASK_SET_STATE_TYPE:
            case ZUC_TASK_PLAN_INIT_TYPE:
            case ZUC_TASK_PLAN_OPEN_TYPE:
            case ZUC_TASK_PLAN_CLOSE_TYPE:
            case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
            case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
            case ZUC_TASK_ABORT_TYPE:
            case ZUC_MOTION_ABORT_TYPE:
            case ZUC_TASK_PLAN_SYNCH_TYPE:
            case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
            case ZUC_AUX_INPUT_WAIT_TYPE:
            case ZUC_MOTION_SET_DOUT_TYPE:
            case ZUC_MOTION_ADAPTIVE_TYPE:
            case ZUC_MOTION_SET_AOUT_TYPE:
            case ZUC_MOTION_EXIT_PROTECT_TYPE:
            case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
            case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
            case ZUC_MOTION_REDUCE_MODE_TYPE:
            case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
            case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
            case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
            case ZUC_MOTION_KINE_INVERSE_TYPE:
            case ZUC_MOTION_KINE_FORWARD_TYPE:
            case ZUC_MOTION_SET_FUNCDI_TYPE:
            case ZUC_MOTION_SET_FUNCDO_TYPE:
            case ZUC_MOTION_SET_SAFETY_FUNCDI_TYPE:
            case ZUC_MOTION_SET_SAFETY_FUNCDO_TYPE:
            case ZUC_MOTION_SET_HOME_VEL_TYPE:
            case ZUC_TRAJ_RIGID_TAP_TYPE:
            case ZUC_TRAJ_SET_TELEOP_ENABLE_TYPE:
            case ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE_TYPE:
            case ZUC_TRAJ_SET_ADMITOP_ENABLE_TYPE:
            case ZUC_TRAJ_SET_COMPLIANCE_ENABLE_TYPE:
            case ZUC_TRAJ_SET_ADMITTANCE_FRAME_TYPE:
            case ZUC_TRAJ_DISABLE_FORCE_CONTROL_TYPE:
            case ZUC_TRAJ_SET_ADMITTANCE_OPTION_TYPE:
            case ZUC_TRAJ_SET_ADMITTANCE_CONFIG_TYPE:
            case ZUC_TRAJ_SET_END_FORCE_CONDITION_TYPE:
            case ZUC_SET_SAFE_ZONE_TYPE:
            case ZUC_SET_SAFE_ATTITUDE_TYPE:
            case ZUC_SET_PLANNER_JERK_TYPE:
            case ZUC_ROBOT_SAFETY_TYPE:
            case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
            case Robot_7_DOF_INV_INFORM_TYPE:
            case Robot_SET_FULL_DH_FLAG_TYPE:
            case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
            case ZUC_REDUCE_SAFETY_TYPE:
            case ZUC_DRAG_SPEED_LIMIT_TYPE:
            case ZUC_JOINT_SAFETY_TYPE:
            case ZUC_SET_DEBUG_TYPE:
            case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
            case ZUC_SET_MUTI_USER_OFFSET_TYPE:
            case ZUC_SET_TOOL_ID_TYPE:
            case ZUC_TORQUE_CONTROL_ENABLE_TYPE:
            case ZUC_SET_USER_ID_TYPE:
            case ZUC_CONVEYOR_ENABLE_TYPE:
            case ZUC_CONVEYOR_DISABLE_TYPE:
            case ZUC_TASK_SET_AUTOWORK_TYPE:
            case ZUC_SET_ROBOT_NAME_TYPE:
            case ZUC_SET_CABINET_ID_TYPE:
            case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE:
            case ZUC_SET_CUSTOM_POSE_TYPE:
            case ZUC_SET_SERVOJ_PARAMETER_TYPE:
            case ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_STARTPOS_TYPE:
            case ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_ENDPOS_TYPE:
            case ZUC_TASK_RESET_PAYLOAD_IDENTIFY_STATUS_TYPE:
            case ZUC_IDENTIFY_CYCLE_TIME_TYPE:
            case ZUC_STOP_IDENTIFY_FRICTION_TYPE:
            case ZUC_START_IDENTIFY_FRICTION_TYPE:
            case ZUC_IDENTIFY_FRICTION_END_POS_TYPE:
            case ZUC_IDENTIFY_FRICTION_START_POS_TYPE:
            case ZUC_SERVO_DYNAMICS_PARA_TYPE:
            case ZUC_SERVO_FRICTION_TYPE:
            case ZUC_COLLISION_SENSITIVITY_TYPE:
            case ZUC_BACKDRIVE_ON_TYPE:
            case ZUC_FORCE_LIMIT_VALUE_TYPE:
            case ZUC_COLLISION_DETECTION_ENABLE_TYPE:
            case ZUC_RS485_MESSAGE_TYPE:
            case ZUC_TIO_RS485_SIG_PROG_STAT_TYPE:
            case ZUC_GRAVITY_DIRECTION_TYPE:
            case ZUC_DRAG_CONTROL_ENABLE_TYPE:
            case ZUC_DRAG_CONTROL_FC_COMPENSATE_TYPE:
            case ZUC_DRAG_CONTROL_FV_COMPENSATE_TYPE:
            case ZUC_DRAG_CONTROL_FILTER_FACTOR_TYPE:
            case ZUC_DRAG_CONTROL_DITHER_FACTOR_TYPE:
            case ZUC_TASK_START_DYNAMICS_IDENTIFY_TYPE:
            case ZUC_TASK_STOP_DYNAMICS_IDENTIFY_TYPE:
            case ZUC_SERVO_UPGRADE_TYPE:
            case ZUC_EXTIO_SETUP_TYPE:
            case ZUC_SETUP_MODBUS_SLAVE_TYPE:
            case ZUC_EXTIO_SETMODE_TYPE:
            case ZUC_SET_MAINTENANCE_MODE_TYPE:
            case ZUC_TORQSENSOR_SETCOMM_TYPE:
            case ZUC_TORQSENSOR_SETPAYLOAD_TYPE:
            case ZUC_TOOL_SETPAYLOAD_TYPE:
            case ZUC_TORQUE_SENSOR_SOFT_LIMIT_TYPE:
            case ZUC_ADMITTANCE_DRAG_DEAD_ZONE_TYPE:
            case ZUC_TORQUE_SENSOR_FILTER_TYPE:
            case ZUC_TORQSENSOR_SETMODE_TYPE:
            case ZUC_TORQSENSOR_SETBRAND_TYPE:
            case ZUC_TASK_CALC_USER_TYPE:
            case ZUC_TASK_CALC_TCP_TYPE:
            case ZUC_TASK_CALC_SAFEATT_TYPE:
            case ZUC_SET_SERVO_PARAM_TYPE:
            case ZUC_SHUT_DOWN_TYPE:
            case ZUC_MOTION_SETUP_IO_CHANNEL_TYPE:
            case ZUC_MOTION_SET_BR_STRT_VOLTAGE_TYPE:
            case ZUC_MOTION_ONE_KEY_OP_TYPE:
            case ZUC_MBTIO_ADD_SIGNAL_TYPE:
            case ZUC_MBTIO_DEL_SIGNAL_TYPE:
            case ZUC_MBTIO_UPDATE_SIGNAL_TYPE:
            case ZUC_MBTIO_SEND_COMMAND_TYPE:
            case ZUC_TASK_SETUP_TIO_RS485_MODE_TYPE:
            case ZUC_TASK_SETUP_TIO_RS485_COMM_TYPE:
            case ZUC_TASK_SETUP_TIO_PIN_MODE_TYPE:
            case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE:
            case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE:
            case ZUC_MOTION_SET_CAPTURE_TYPE:
            case ZUC_STEP_STATUS_TYPE:
            case ZUC_MOTION_CLEAR_CAPTURE_TYPE:
            case ZUC_SET_NET_EXCEPTION_HANDLE_TYPE:
            case ZUC_SET_COLLISION_OPTION_TYPE:
            case ZUC_SET_CABVOLTAGE_TYPE:
            case ZUC_SET_SIM_ENABLE_TYPE:
            case ZUC_SET_SIM_ROBOT_TYPE:
            case ZUC_SYNC_ZUCSETTINGS_TYPE:
            case ZUC_TRAJ_SET_PAYLOAD_TYPE:
            case ZUC_SET_ROBOT_PAYLOAD_TYPE:
            case ZUC_CLEAR_ERROR_TYPE:
            case ZUC_SET_BASE_OFFSET_TYPE:
                retval = zucTaskIssueCommand(zucCommand);
                break;
            case ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET_TYPE:
            case ZUC_TRAJ_SET_ROBOT_USER_FRAME_TYPE:
                retval = 0;
                if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_FREE)
                {
                    retval = zucTaskIssueCommand(zucCommand);
                }
                break;
            case ZUC_TRAJ_CALIB_DHPARAM_TYPE:
                zucTaskQueueCommand(zucCommand);
                zucTaskPlanSetWait();                    // signify no more reading
                zucTaskQueueCommand(&taskPlanSynchCmd);  // then resynch interpreter
                break;
            case ZUC_TASK_PLAN_PAUSE_TYPE:
                if (zucStatus->motion.category_one_stop_program_pause == 1)
                {
                    retval = zucTaskIssueCommand(zucCommand);
                }
                break;
            default:
                zucOperatorError(ROBOT_NOT_ENABLED, _("command (%d) cannot be executed until the machine is enabled"), type);
                retval = -1;
                break;
            }  // switch (type)
        default:
            break;
        }  // switch (mode)

        break;  // case ZUC_TASK_STATE_OFF,ESTOP,ESTOP_RESET

    case ZUC_TASK_STATE_ON:
        switch (zucStatus->task.mode)
        {
        case ZUC_TASK_MODE_MANUAL:  // ON, MANUAL
            switch (type)
            {
            case 0:
            case ZUC_NULL_TYPE:
                break;
            case ZUC_TASK_PLAN_EXECUTE_TYPE:
                // resynch the interpreter for external move
                zucTaskIssueCommand(&taskPlanSynchCmd);
                retval = zucTaskIssueCommand(zucCommand);
                break;
            // commands need to be queuesZUC_SET_MUTI_TOOL_OFFSET_TYPE:ZUC_SET_MUTI_TOOL_OFFSET_TYPE:ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
            case ZUC_TRAJ_CALIB_DHPARAM_TYPE:
            case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
            case ZUC_SET_TOOL_ID_TYPE:
            case ZUC_SET_MUTI_USER_OFFSET_TYPE:
            case ZUC_SET_USER_ID_TYPE:
            case ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET_TYPE:
            case ZUC_TRAJ_SET_ROBOT_USER_FRAME_TYPE:
                zucTaskQueueCommand(zucCommand);
                //zucTaskPlanSetWait();
                zucTaskQueueCommand(&taskPlanSynchCmd);
                break;
            // commands in admittance mode
            case ZUC_TORQSENSOR_SETCOMM_TYPE:
            case ZUC_SHUT_DOWN_TYPE:
            case ZUC_TORQSENSOR_SETPAYLOAD_TYPE:
            case ZUC_TOOL_SETPAYLOAD_TYPE:
            case ZUC_TORQUE_SENSOR_SOFT_LIMIT_TYPE:
            case ZUC_TORQUE_SENSOR_FILTER_TYPE:
            case ZUC_TORQSENSOR_SETBRAND_TYPE:
            case ZUC_TORQSENSOR_SETMODE_TYPE:
            case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE:
            case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE:
            case ZUC_MBTIO_SEND_COMMAND_TYPE:
            case ZUC_TASK_PLAN_STEP_TYPE:
            case ZUC_SET_NET_EXCEPTION_HANDLE_TYPE:
                if (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_ADMITTANCE)
                {
                    zucTaskIssueCommand(zucCommand);
                    break;
                }
            case ZUC_SET_SIM_ENABLE_TYPE:
            case ZUC_SET_SIM_ROBOT_TYPE:
            default:
                if (allow_while_idle_type())
                {
                    retval = zucTaskIssueCommand(zucCommand);
                    break;
                }
                zucOperatorError(OPERATION_IN_WRONG_MODE,
                                 _("can't do that (%s:%d) in manual mode#{\"fixed_key\":[\"ON_MANUAL\",%d]}"),
                                 zucCommand->msg_name(),
                                 (int)type,
                                 (int)type);

                retval = -1;
                break;
            }                     // switch (type) in ON, MANUAL
            break;                // case ZUC_TASK_MODE_MANUAL
        case ZUC_TASK_MODE_AUTO:  // ON, AUTO
            switch (zucStatus->task.interpState)
            {
            case ZUC_TASK_INTERP_IDLE:  // ON, AUTO, IDLE
                switch (type)
                {
                case 0:
                case ZUC_NULL_TYPE:
                    break;
                // immediate commands
                case ZUC_TASK_PLAN_RUN_TYPE:
                case ZUC_TASK_PLAN_EXECUTE_TYPE:
                    retval = zucTaskIssueCommand(zucCommand);
                    break;
                case ZUC_TASK_PLAN_STEP_TYPE:
                    taskPlanRunCmd.line = 1;  // run from start
                    retval = zucTaskIssueCommand(&taskPlanRunCmd);
                    if (retval != 0)
                        break;
                    // zucTrajPause();
                    // robot_motion_task->do_pause();
                    if (zucStatus->task.interpState != ZUC_TASK_INTERP_PAUSED)
                    {
                        interpResumeState = zucStatus->task.interpState;
                    }
                    zucStatus->task.interpState = ZUC_TASK_INTERP_PAUSED;
                    zucStatus->task.task_paused = 1;
                    retval = 0;
                    stepping = 1;
                    zucStatus->stepStatus.isSteping = 1;
                    steppingLine = zucStatus->motion.traj.id;  // 保存jks文件中当前运动轨迹行号
                    break;
                // queue commands directly affecting motion
                case ZUC_TRAJ_CALIB_DHPARAM_TYPE:
                case ZUC_SET_TOOL_ID_TYPE:
                case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
                case ZUC_SET_USER_ID_TYPE:
                case ZUC_SET_MUTI_USER_OFFSET_TYPE:
                case ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET_TYPE:
                case ZUC_STEP_STATUS_TYPE:
                case ZUC_TRAJ_SET_ROBOT_USER_FRAME_TYPE:
                case ZUC_CONVEYOR_DISABLE_TYPE:
                    zucTaskQueueCommand(zucCommand);
                    //zucTaskPlanSetWait();
                    zucTaskQueueCommand(&taskPlanSynchCmd);
                    break;
                case ZUC_CONVEYOR_ENABLE_TYPE:
                    zucTaskQueueCommand(&taskPlanSynchCmd);
                    zucTaskQueueCommand(zucCommand);
                    break;
                default:
                    if (allow_while_idle_type())
                    {
                        retval = zucTaskIssueCommand(zucCommand);
                        break;
                    }
                    zucOperatorError(OPERATION_IN_WRONG_MODE,
                                     _("can't do that (%s) in auto mode with the interpreter idle#{\"fixed_key\":[\"ON_AUTO_IDLE\",\"%s\"]}"),
                                     zucCommand->msg_name(),
                                     zucCommand->msg_name());
                    retval = -1;
                    break;
                }                          // switch (type) in ON, AUTO, IDLE
                break;                     // ZUC_TASK_INTERP_IDLE
            case ZUC_TASK_INTERP_READING:  // ON, AUTO, READING
                switch (type)
                {
                case 0:
                case ZUC_NULL_TYPE:
                    break;
                // immediate commands
                case ZUC_TRAJ_PAUSE_TYPE:
                case ZUC_TRAJ_RESUME_TYPE:
                case ZUC_TRAJ_ABORT_TYPE:
                case ZUC_TRAJ_SET_SCALE_TYPE:
                case ZUC_TRAJ_SET_RAPID_SCALE_TYPE:
                case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE:
                case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
                case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
                case ZUC_TASK_PLAN_PAUSE_TYPE:
                case ZUC_TASK_PLAN_RESUME_TYPE:
                case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
                case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
                case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
                case ZUC_TASK_PLAN_END_TYPE:
                case ZUC_TASK_SET_MODE_TYPE:
                case ZUC_TASK_SET_OPERATION_MODE_TYPE:
                case ZUC_TASK_SET_STATE_TYPE:
                case ZUC_AXIS_CMD_POWER_TYPE:
                case ZUC_AXIS_CMD_RESET_TYPE:
                case ZUC_TASK_ABORT_TYPE:
                case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
                case ZUC_AUX_INPUT_WAIT_TYPE:
                case ZUC_TRAJ_RIGID_TAP_TYPE:
                case ZUC_SET_DEBUG_TYPE:
                case ZUC_SET_SERVO_PARAM_TYPE:
                case ZUC_TRAJ_SET_PAYLOAD_TYPE:
                case ZUC_SET_ROBOT_PAYLOAD_TYPE:
                case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
                case ZUC_SET_TOOL_ID_TYPE:
                case ZUC_TORQUE_CONTROL_ENABLE_TYPE:
                case ZUC_SET_MUTI_USER_OFFSET_TYPE:
                case ZUC_SET_USER_ID_TYPE:
                case ZUC_TASK_CALC_TCP_TYPE:
                case ZUC_TASK_CALC_USER_TYPE:
                case ZUC_TASK_SET_AUTOWORK_TYPE:
                case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE:
                case ZUC_TRAJ_SET_ADMITTANCE_ENABLE_TYPE:
                case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
                case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
                case ZUC_MOTION_REDUCE_MODE_TYPE:
                case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_DOUT_TYPE:
                case ZUC_MOTION_SET_AOUT_TYPE:
                case ZUC_CONVEYOR_ENABLE_TYPE:
                case ZUC_MOTION_EXIT_PROTECT_TYPE:
                case ZUC_MOTION_ONE_KEY_OP_TYPE:
                case ZUC_MBTIO_ADD_SIGNAL_TYPE:
                case ZUC_MBTIO_DEL_SIGNAL_TYPE:
                case ZUC_MBTIO_UPDATE_SIGNAL_TYPE:
                case ZUC_MBTIO_SEND_COMMAND_TYPE:
                case ZUC_MOTION_SET_CAPTURE_TYPE:
                case ZUC_ROBOT_SAFETY_TYPE:
                case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
                case Robot_7_DOF_INV_INFORM_TYPE:
                case Robot_SET_FULL_DH_FLAG_TYPE:
                case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
                case ZUC_JOINT_SAFETY_TYPE:
                case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE:
                case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE:
                case ZUC_STEP_STATUS_TYPE:
                case ZUC_MOTION_CLEAR_CAPTURE_TYPE:
                case ZUC_TRAJ_SET_TELEOP_ENABLE_TYPE:
                case ZUC_SET_NET_EXCEPTION_HANDLE_TYPE:
                case ZUC_APP_SET_CONNECTSTAT_TYPE:
                    retval = zucTaskIssueCommand(zucCommand);
                    return retval;
                    break;
                case ZUC_CONVEYOR_DISABLE_TYPE:
                    zucTaskIssueCommand(zucCommand);
                    zucTaskQueueCommand(&taskPlanSynchCmd);
                    break;
                case ZUC_TASK_PLAN_STEP_TYPE: {
                    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
                    if (robot_motion_task.get())
                    {
                        break;
                    }
                    robot_motion_task->zucTrajStep();
                    stepping = 1;      // set stepping mode in case it's not
                    steppingWait = 0;  // clear the wait
                    zucStatus->task.task_paused = 1;
                    steppingLine = zucStatus->motion.traj.id;  // 保存jks文件中当前运动轨迹行号
                    break;
                }

                default:
                    zucOperatorError(OPERATION_IN_WRONG_MODE,
                                     _("can't do that (%s) in auto mode with the interpreter reading#{\"fixed_key\":[\"READING\",\"%s\"]}"),
                                     zucCommand->msg_name(),
                                     zucCommand->msg_name());
                    retval = -1;
                    break;
                }  // switch (type) in ON, AUTO, READING
                readahead_reading();
                break;                    // ZUC_TASK_INTERP_READING
            case ZUC_TASK_INTERP_PAUSED:  // ON, AUTO, PAUSED
                switch (type)
                {
                case 0:
                case ZUC_NULL_TYPE:
                    break;
                // immediate commands
                case ZUC_TRAJ_PAUSE_TYPE:
                case ZUC_TRAJ_RESUME_TYPE:
                case ZUC_TRAJ_ABORT_TYPE:
                case ZUC_TRAJ_SET_SCALE_TYPE:
                case ZUC_TRAJ_SET_RAPID_SCALE_TYPE:
                case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE:
                case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
                case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
                case ZUC_TASK_SET_MODE_TYPE:
                case ZUC_TASK_SET_OPERATION_MODE_TYPE:
                case ZUC_TASK_SET_STATE_TYPE:
                case ZUC_AXIS_CMD_POWER_TYPE:
                case ZUC_AXIS_CMD_RESET_TYPE:
                case ZUC_TASK_ABORT_TYPE:
                case ZUC_TASK_PLAN_EXECUTE_TYPE:
                case ZUC_TASK_PLAN_PAUSE_TYPE:
                case ZUC_TASK_PLAN_RESUME_TYPE:
                case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
                case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
                case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
                case ZUC_TASK_PLAN_END_TYPE:
                case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
                case ZUC_AUX_INPUT_WAIT_TYPE:
                case ZUC_TRAJ_RIGID_TAP_TYPE:
                case ZUC_SET_DEBUG_TYPE:
                case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
                case ZUC_SET_TOOL_ID_TYPE:
                case ZUC_TORQUE_CONTROL_ENABLE_TYPE:
                case ZUC_SET_MUTI_USER_OFFSET_TYPE:
                case ZUC_SET_USER_ID_TYPE:
                case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
                case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
                case ZUC_TASK_CALC_TCP_TYPE:
                case ZUC_TASK_CALC_USER_TYPE:
                case ZUC_MOTION_REDUCE_MODE_TYPE:
                case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_DOUT_TYPE:
                case ZUC_MOTION_SET_AOUT_TYPE:
                case ZUC_MOTION_EXIT_PROTECT_TYPE:
                case ZUC_CONVEYOR_ENABLE_TYPE:
                case ZUC_MOTION_ONE_KEY_OP_TYPE:
                case ZUC_MOTION_SET_CAPTURE_TYPE:
                case ZUC_MOTION_CLEAR_CAPTURE_TYPE:
                case ZUC_MOTION_SET_HOME_VEL_TYPE:
                case ZUC_SET_SAFE_ZONE_TYPE:
                case ZUC_SET_SAFE_ATTITUDE_TYPE:
                case ZUC_SET_PLANNER_JERK_TYPE:
                case ZUC_ROBOT_SAFETY_TYPE:
                case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
                case Robot_7_DOF_INV_INFORM_TYPE:
                case Robot_SET_FULL_DH_FLAG_TYPE:
                case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
                case ZUC_JOINT_SAFETY_TYPE:
                case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE:
                case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE:
                case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE:
                case ZUC_APP_SET_CONNECTSTAT_TYPE:
                    retval = zucTaskIssueCommand(zucCommand);
                    break;
                case ZUC_CONVEYOR_DISABLE_TYPE:
                    zucTaskIssueCommand(zucCommand);
                    zucTaskQueueCommand(&taskPlanSynchCmd);
                    break;
                case ZUC_STEP_STATUS_TYPE:
                    retval = zucTaskIssueCommand(zucCommand);
                    break;
                case ZUC_TASK_PLAN_STEP_TYPE: {
                    stepping = 1;
                    steppingWait = 0;
                    // if (/* zucStatus->motion.traj.paused && zucStatus->motion.traj.queue > 0 */zucStatus->task.task_paused) {
                    if (zucStatus->motion.traj.paused && zucStatus->motion.traj.queue > 0 /* zucStatus->task.task_paused */)
                    {
                        auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
                        if (robot_motion_task.get())
                        {
                            break;
                        }
                        // there are pending motions paused; step them
                        robot_motion_task->zucTrajStep();
                    }
                    else
                    {
                        zucStatus->task.interpState = (enum ZUC_TASK_INTERP_ENUM)interpResumeState;
                    }
                    zucStatus->task.task_paused = 1;
                    steppingLine = zucStatus->motion.traj.id;  // 保存jks文件中当前运动轨迹行号
                    break;
                }
                default:
                    zucOperatorError(OPERATION_IN_WRONG_MODE,
                                     _("can't do that (%s) in auto mode with the interpreter paused#{\"fixed_key\":[\"ON_AUTO_PAUSED\",\"%s\"]}"),
                                     zucCommand->msg_name(),
                                     zucCommand->msg_name());
                    retval = -1;
                    break;
                }  // switch (type) in ON, AUTO, PAUSED

                break;  // ZUC_TASK_INTERP_PAUSED

            case ZUC_TASK_INTERP_WAITING:
                // interpreter ran to end,handle input commands
                switch (type)
                {
                case 0:
                case ZUC_NULL_TYPE:
                    break;
                // immediate commands
                case ZUC_TRAJ_PAUSE_TYPE:
                case ZUC_TRAJ_RESUME_TYPE:
                case ZUC_TRAJ_ABORT_TYPE:
                case ZUC_TRAJ_SET_SCALE_TYPE:
                case ZUC_TRAJ_SET_RAPID_SCALE_TYPE:
                case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE:
                case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
                case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
                case ZUC_TASK_PLAN_EXECUTE_TYPE:
                case ZUC_TASK_PLAN_PAUSE_TYPE:
                case ZUC_TASK_PLAN_RESUME_TYPE:
                case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
                case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
                case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
                case ZUC_TASK_PLAN_END_TYPE:
                case ZUC_TASK_SET_MODE_TYPE:
                case ZUC_TASK_SET_OPERATION_MODE_TYPE:
                case ZUC_TASK_SET_STATE_TYPE:
                case ZUC_AXIS_CMD_POWER_TYPE:
                case ZUC_AXIS_CMD_RESET_TYPE:
                case ZUC_TASK_ABORT_TYPE:
                case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
                case ZUC_AUX_INPUT_WAIT_TYPE:
                case ZUC_TRAJ_RIGID_TAP_TYPE:
                case ZUC_SET_DEBUG_TYPE:
                case ZUC_SET_SERVO_PARAM_TYPE:
                case ZUC_TRAJ_SET_PAYLOAD_TYPE:
                case ZUC_SET_ROBOT_PAYLOAD_TYPE:
                case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
                case ZUC_SET_TOOL_ID_TYPE:
                case ZUC_TORQUE_CONTROL_ENABLE_TYPE:
                case ZUC_SET_MUTI_USER_OFFSET_TYPE:
                case ZUC_SET_USER_ID_TYPE:
                case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
                case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
                case ZUC_TASK_CALC_TCP_TYPE:
                case ZUC_TASK_CALC_USER_TYPE:
                case ZUC_MOTION_REDUCE_MODE_TYPE:
                case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
                case ZUC_MOTION_SET_DOUT_TYPE:
                case ZUC_MOTION_SET_AOUT_TYPE:
                case ZUC_MOTION_EXIT_PROTECT_TYPE:
                case ZUC_CONVEYOR_ENABLE_TYPE:
                case ZUC_MOTION_ONE_KEY_OP_TYPE:
                case ZUC_MBTIO_ADD_SIGNAL_TYPE:
                case ZUC_MBTIO_DEL_SIGNAL_TYPE:
                case ZUC_MBTIO_UPDATE_SIGNAL_TYPE:
                case ZUC_MBTIO_SEND_COMMAND_TYPE:
                case ZUC_MOTION_SET_CAPTURE_TYPE:
                case ZUC_MOTION_SET_HOME_VEL_TYPE:
                case ZUC_ROBOT_SAFETY_TYPE:
                case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
                case Robot_7_DOF_INV_INFORM_TYPE:
                case Robot_SET_FULL_DH_FLAG_TYPE:
                case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
                case ZUC_JOINT_SAFETY_TYPE:
                case ZUC_MOTION_CLEAR_CAPTURE_TYPE:
                case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE:
                case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE:
                case ZUC_STEP_STATUS_TYPE:
                case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE:
                case ZUC_APP_SET_CONNECTSTAT_TYPE:
                    retval = zucTaskIssueCommand(zucCommand);
                    break;
                case ZUC_CONVEYOR_DISABLE_TYPE:
                    zucTaskIssueCommand(zucCommand);
                    zucTaskQueueCommand(&taskPlanSynchCmd);
                    break;
                case ZUC_TASK_PLAN_STEP_TYPE: {
                    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
                    if (robot_motion_task.get())
                    {
                        break;
                    }
                    robot_motion_task->zucTrajStep();
                    stepping = 1;                              // set stepping mode in case it's not
                    steppingWait = 0;                          // clear the wait
                    steppingLine = zucStatus->motion.traj.id;  // 保存jks文件中当前运动轨迹行号
                    break;
                }
                default:
                    zucOperatorError(OPERATION_IN_WRONG_MODE,
                                     _("can't do that (%s) in auto mode with the interpreter waiting#{\"fixed_key\":[\"ON_AUTO_WAIT\",\"%s\"]}"),
                                     zucCommand->msg_name(),
                                     zucCommand->msg_name());
                    retval = -1;
                    break;
                }  // switch (type) in ON, AUTO, WAITING
                // handle interp readahead logic
                readahead_waiting();
                break;  // end of case ZUC_TASK_INTERP_WAITING
            default:
                // coding error
                rcs_print_error("invalid mode(%d)", zucStatus->task.mode);
                retval = -1;
                break;
            }       // switch (mode) in ON, AUTO
            break;  // case ZUC_TASK_MODE_AUTO

        case ZUC_TASK_MODE_MDI:  // ON, MDI
            switch (type)
            {
            case 0:
            case ZUC_NULL_TYPE:
                break;
            // immediate commands
            case ZUC_TRAJ_SET_SCALE_TYPE:
            case ZUC_TRAJ_SET_RAPID_SCALE_TYPE:
            case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE:
            case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
            case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
            case ZUC_TASK_SET_MODE_TYPE:
            case ZUC_TASK_SET_OPERATION_MODE_TYPE:
            case ZUC_TASK_SET_STATE_TYPE:
            case ZUC_AXIS_CMD_POWER_TYPE:
            case ZUC_AXIS_CMD_RESET_TYPE:
            case ZUC_TASK_PLAN_INIT_TYPE:
            case ZUC_TASK_PLAN_OPEN_TYPE:
            case ZUC_TASK_PLAN_CLOSE_TYPE:
            case ZUC_TASK_PLAN_PAUSE_TYPE:
            case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
            case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
            case ZUC_TASK_PLAN_RESUME_TYPE:
            case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
            case ZUC_TASK_PLAN_END_TYPE:
            case ZUC_TASK_PLAN_SYNCH_TYPE:
            case ZUC_TASK_ABORT_TYPE:
            case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
            case ZUC_AUX_INPUT_WAIT_TYPE:
            case ZUC_MOTION_SET_DOUT_TYPE:
            case ZUC_MOTION_SET_AOUT_TYPE:
            case ZUC_MOTION_ADAPTIVE_TYPE:
            case ZUC_MOTION_EXIT_PROTECT_TYPE:
            case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
            case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
            case ZUC_MOTION_REDUCE_MODE_TYPE:
            case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
            case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
            case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
            case ZUC_MOTION_KINE_INVERSE_TYPE:
            case ZUC_MOTION_KINE_FORWARD_TYPE:
            case ZUC_MOTION_SET_HOME_VEL_TYPE:
            case ZUC_TRAJ_RIGID_TAP_TYPE:
            case ZUC_SET_DEBUG_TYPE:
            case ZUC_SET_SERVO_PARAM_TYPE:
            case ZUC_CONVEYOR_ENABLE_TYPE:
            case ZUC_CONVEYOR_DISABLE_TYPE:
            case ZUC_MOTION_ONE_KEY_OP_TYPE:
            case ZUC_MOTION_SET_CAPTURE_TYPE:
            case ZUC_MOTION_CLEAR_CAPTURE_TYPE:
            case ZUC_SET_SAFE_ZONE_TYPE:
            case ZUC_SET_SAFE_ATTITUDE_TYPE:
            case ZUC_SET_PLANNER_JERK_TYPE:
            case ZUC_ROBOT_SAFETY_TYPE:
            case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
            case Robot_7_DOF_INV_INFORM_TYPE:
            case Robot_SET_FULL_DH_FLAG_TYPE:
            case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
            case ZUC_STEP_STATUS_TYPE:
            case ZUC_JOINT_SAFETY_TYPE:
            case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE:
            case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE:
            case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE:
            case ZUC_APP_SET_CONNECTSTAT_TYPE:
            case ZUC_TRAJ_SET_PAYLOAD_TYPE:
            case ZUC_SET_ROBOT_PAYLOAD_TYPE:
                retval = zucTaskIssueCommand(zucCommand);
                break;
            case ZUC_TASK_PLAN_EXECUTE_TYPE:
                if ((mdi_execute_queue.len() == 0) && (interp_list.len() == 0) && (zucTaskCommand == NULL) && (zucTaskPlanIsWait() == 0))
                {
                    retval = zucTaskIssueCommand(zucCommand);
                }
                else
                {
                    mdi_execute_queue.append(zucCommand);
                    zucStatus->task.queuedMDIcommands = mdi_execute_queue.len();
                    retval = 0;
                }
                break;
            case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
            case ZUC_SET_TOOL_ID_TYPE:
            case ZUC_SET_MUTI_USER_OFFSET_TYPE:
            case ZUC_SET_USER_ID_TYPE:
            case ZUC_TRAJ_CALIB_DHPARAM_TYPE:
                zucTaskQueueCommand(zucCommand);
                zucTaskPlanSetWait();
                zucTaskQueueCommand(&taskPlanSynchCmd);
                break;
            default:
                if (allow_while_idle_type())
                {
                    retval = zucTaskIssueCommand(zucCommand);
                    break;
                }
                zucOperatorError(OPERATION_IN_WRONG_MODE,
                                 _("can't do that (%s:%d) in MDI mode#{\"fixed_key\":[\"ON_MDI\",\"%s\"]}"),
                                 zucCommand->msg_name(),
                                 (int)type,
                                 zucCommand->msg_name());
                retval = -1;
                break;
            }  // switch (type) in ON, MDI
            mdi_execute_hook();
            break;  // case ZUC_TASK_MODE_MDI

        case ZUC_TASK_MODE_DRAG:  // ON, DRAG
            switch (type)
            {
            case 0:
            case ZUC_NULL_TYPE:
                break;
            // immediate commands
            case ZUC_TRAJ_SET_SCALE_TYPE:
            case ZUC_TRAJ_SET_RAPID_SCALE_TYPE:
            case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE:
            case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
            case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
            case ZUC_TASK_ABORT_TYPE:
            case ZUC_MOTION_ABORT_TYPE:
            case ZUC_TASK_PLAN_OPEN_TYPE:
            case ZUC_TASK_PLAN_CLOSE_TYPE:
            case ZUC_TASK_PLAN_INIT_TYPE:
            case ZUC_TASK_PLAN_SYNCH_TYPE:
            case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
            case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
            case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
            case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
            case ZUC_AUX_INPUT_WAIT_TYPE:
            case ZUC_MOTION_SET_DOUT_TYPE:
            case ZUC_MOTION_SET_AOUT_TYPE:
            case ZUC_MOTION_ADAPTIVE_TYPE:
            case ZUC_MOTION_EXIT_PROTECT_TYPE:
            case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
            case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
            case ZUC_MOTION_REDUCE_MODE_TYPE:
            case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
            case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
            case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
            case ZUC_MOTION_KINE_INVERSE_TYPE:
            case ZUC_MOTION_KINE_FORWARD_TYPE:
            case ZUC_MOTION_SET_HOME_VEL_TYPE:
            case ZUC_SET_DEBUG_TYPE:
            case ZUC_TASK_SET_MODE_TYPE:
            case ZUC_TASK_SET_OPERATION_MODE_TYPE:
            case ZUC_CONVEYOR_ENABLE_TYPE:
            case ZUC_CONVEYOR_DISABLE_TYPE:
            case ZUC_MOTION_ONE_KEY_OP_TYPE:
            case ZUC_STEP_STATUS_TYPE:
            case ZUC_ROBOT_SAFETY_TYPE:
            case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE:
            case ZUC_APP_SET_CONNECTSTAT_TYPE:
                retval = zucTaskIssueCommand(zucCommand);
                break;
            default:
                zucOperatorError(OPERATION_IN_WRONG_MODE,
                                 _("can't do that (%s:%d) in drag mode#{\"fixed_key\":[\"ON_DRAG\",\"%s\"]}"),
                                 zucCommand->msg_name(),
                                 (int)type,
                                 zucCommand->msg_name());
                retval = -1;
                break;
            }
        default:
            break;
        }  // switch (zucStatus->task.mode)

        break;  // case ZUC_TASK_STATE_ON

    default:
        break;
    }  // switch (task.state)

    return retval;
}

/*
 zucTaskCheckPreconditions() is called for commands on the interp_list.
 Immediate commands, i.e., commands sent from calls to zucTaskIssueCommand()
 in zucTaskPlan() directly, are not handled here.

 The return value is a state for zucTaskExecute() to wait on, e.g.,
 ZUC_TASK_EXEC_WAITING_FOR_MOTION, before the command can be sent out.
 */
static int zucTaskCheckPreconditions(NMLmsg* cmd)
{
    if (0 == cmd)
    {
        return ZUC_TASK_EXEC_DONE;
    }

    switch (cmd->type)
    {
    // operator messages, if queued, will go out when everything before
    // them is done
    case ZUC_OPERATOR_ERROR_TYPE:
    case ZUC_OPERATOR_TEXT_TYPE:
    case ZUC_OPERATOR_DISPLAY_TYPE:
    case ZUC_SYSTEM_CMD_TYPE:
    case ZUC_TRAJ_RIGID_TAP_TYPE:  //and this
    case ZUC_AUX_INPUT_WAIT_TYPE:
        return ZUC_TASK_EXEC_WAITING_FOR_MOTION;
        break;

    case ZUC_TRAJ_LINEAR_MOVE_TYPE:
    case ZUC_TOPPRA_FINISH_LEFT_TC_TYPE:
    case ZUC_TOPPRA_FIRST_COMMAND_TYPE:
    case ZUC_TRAJ_CIRCULAR_MOVE_TYPE:
    case ZUC_TRAJ_JOINT_MOVE_TYPE:
    case ZUC_TRAJ_MOVC_TYPE:
    case ZUC_TRAJ_SET_ACCELERATION_TYPE:
    case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
    case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
    case ZUC_TRAJ_MOVS_TYPE:
        return ZUC_TASK_EXEC_WAITING_FOR_IO;
        break;

    case ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET_TYPE:
    case ZUC_TRAJ_SET_ROBOT_USER_FRAME_TYPE:
    case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
    case ZUC_SET_TOOL_ID_TYPE:
    case ZUC_SET_MUTI_USER_OFFSET_TYPE:
    case ZUC_SET_USER_ID_TYPE:
    case ZUC_CONVEYOR_ENABLE_TYPE:
    case ZUC_CONVEYOR_DISABLE_TYPE:
    case ZUC_TRAJ_SET_PAYLOAD_TYPE:
    case ZUC_SET_ROBOT_PAYLOAD_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_CONFIG_TYPE:
    case ZUC_TRAJ_SET_END_FORCE_CONDITION_TYPE:
    case ZUC_TRAJ_SET_COMPLIANCE_ENABLE_TYPE:
    case ZUC_TORQUE_CONTROL_ENABLE_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_FRAME_TYPE:
    case ZUC_TRAJ_DISABLE_FORCE_CONTROL_TYPE:
    case ZUC_TRAJ_CALIB_DHPARAM_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_ENABLE_TYPE:
    case ZUC_TASK_PLAN_PAUSE_TYPE:
    case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
    case ZUC_TASK_PLAN_END_TYPE:
    case ZUC_TASK_PLAN_INIT_TYPE:
    case ZUC_TASK_PLAN_RUN_TYPE:
    case ZUC_TASK_PLAN_SYNCH_TYPE:
    case ZUC_TASK_PLAN_EXECUTE_TYPE:
    case ZUC_TRAJ_DELAY_TYPE:
    case ZUC_SET_SERVO_PARAM_TYPE:
    case ZUC_MOTION_ONE_KEY_OP_TYPE:
    case ZUC_MOTION_SET_CAPTURE_TYPE:
    case ZUC_MOTION_CLEAR_CAPTURE_TYPE:
    case ZUC_MBTIO_SEND_COMMAND_TYPE:
    case ZUC_ROBOT_SAFETY_TYPE:
    case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
    case Robot_7_DOF_INV_INFORM_TYPE:
    case Robot_SET_FULL_DH_FLAG_TYPE:
    case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
    case ZUC_REDUCE_SAFETY_TYPE:
    case ZUC_DRAG_SPEED_LIMIT_TYPE:
    case ZUC_MBTIO_UPDATE_SIGNAL_TYPE:
        return ZUC_TASK_EXEC_WAITING_FOR_MOTION;
        break;

    case ZUC_MOTION_SET_AOUT_TYPE:
        if (((ZUC_MOTION_SET_AOUT*)cmd)->now)
        {
            return ZUC_TASK_EXEC_WAITING_FOR_MOTION;
        }
        return ZUC_TASK_EXEC_DONE;
        break;

    case ZUC_MOTION_SET_DOUT_TYPE:
        if (((ZUC_MOTION_SET_DOUT*)cmd)->now)
        {
            return ZUC_TASK_EXEC_WAITING_FOR_MOTION;
        }
        return ZUC_TASK_EXEC_DONE;
        break;

    case ZUC_MOTION_ADAPTIVE_TYPE:
        return ZUC_TASK_EXEC_WAITING_FOR_MOTION;
        break;

    case ZUC_MOTION_EXIT_PROTECT_TYPE:
    case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
    case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
    case ZUC_MOTION_REDUCE_MODE_TYPE:
    case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
    case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
    case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
    case ZUC_MOTION_KINE_INVERSE_TYPE:
    case ZUC_MOTION_KINE_FORWARD_TYPE:
    case ZUC_MOTION_SET_HOME_VEL_TYPE:
    case ZUC_STEP_STATUS_TYPE:
        return ZUC_TASK_EXEC_DONE;
        break;

    default:
        // unrecognized command
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print_error("preconditions: unrecognized command %d:%s\n", (int)cmd->type, zucCommand->msg_name());
        }
        return ZUC_TASK_EXEC_ERROR;
        break;
    }

    return ZUC_TASK_EXEC_DONE;
}

// puts command on interp list
int zucTaskQueueCommand(NMLmsg* cmd)
{
    if (0 == cmd)
    {
        return 0;
    }
    interp_list.append(cmd);

    return 0;
}

// issues command immediately
static int zucTaskIssueCommand(NMLmsg* cmd)
{
    int retval = 0;
    int execRetval = 0;

    if (0 == cmd)
    {
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print("zucTaskIssueCommand() null command\n");
        }
        return 0;
    }
    if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
    {
        rcs_print("Issuing %s -- \t (%s)\n", cmd->msg_name(), zucCommandBuffer->msg2str(cmd));
    }
    switch (cmd->type)
    {
        // general commands

    case ZUC_OPERATOR_ERROR_TYPE:
        retval = zucOperatorError(((ZUC_OPERATOR_ERROR*)cmd)->errcode, "%s", ((ZUC_OPERATOR_ERROR*)cmd)->error);
        break;

    case ZUC_OPERATOR_TEXT_TYPE:
        retval = zucOperatorText(((ZUC_OPERATOR_TEXT*)cmd)->id, "%s", ((ZUC_OPERATOR_TEXT*)cmd)->text);
        break;

    case ZUC_OPERATOR_DISPLAY_TYPE:
        retval = zucOperatorDisplay(((ZUC_OPERATOR_DISPLAY*)cmd)->id, "%s", ((ZUC_OPERATOR_DISPLAY*)cmd)->display);
        break;

    case ZUC_SYSTEM_CMD_TYPE:
        retval = zucSystzucmd(((ZUC_SYSTEM_CMD*)cmd)->string);
        break;

    case ZUC_JOG_CONT_TYPE: {
        jog_cont_msg = (ZUC_JOG_CONT*)cmd;
        retval = zucJogCont(jog_cont_msg->robot_id, jog_cont_msg->joint_or_axis, jog_cont_msg->vel, jog_cont_msg->jjogmode);
        break;
    }
    case ZUC_JOG_STOP_TYPE: {
        jog_stop_msg = (ZUC_JOG_STOP*)cmd;
        retval = zucJogStop(jog_stop_msg->robot_id);
        break;
    }
    case ZUC_JOG_INCR_TYPE: {
        jog_incr_msg = (ZUC_JOG_INCR*)cmd;
        retval = zucJogIncr(jog_incr_msg->robot_id, jog_incr_msg->joint_or_axis, jog_incr_msg->incr, jog_incr_msg->vel, jog_incr_msg->jjogmode);
        break;
    }
    case ZUC_JOG_ABS_TYPE: {
        jog_abs_msg = (ZUC_JOG_ABS*)cmd;
        zucJogAbs(jog_abs_msg->robot_id, jog_abs_msg->joint_or_axis, jog_abs_msg->pos, jog_abs_msg->vel, jog_abs_msg->jjogmode);
        break;
    }
    case ZUC_JOG_MULTI_JOINT_TYPE: {
        auto jog_multi_joint_msg = (ZUC_JOG_MULTI_JOINT*)cmd;
        retval = zucJogMultiJoint(jog_multi_joint_msg->robot_id, jog_multi_joint_msg->vel, jog_multi_joint_msg->pos);
        break;
    }
    case ZUC_JOG_MOVJ_TYPE: {
        movj_incr_msg = (ZUC_JOG_MOVJ*)cmd;
        auto jogmovj = mot::al::MotionProxy::instance().robot(movj_incr_msg->robot_id);
        if (!jogmovj.get())
        {
            break;
        }
        MoveSpeed speed(movj_incr_msg->vel, movj_incr_msg->acc, 0);
        MovjParam movj_param(1, movj_incr_msg->mvOpt, movj_incr_msg->tol, movj_incr_msg->executing_line_id, movj_incr_msg->end_cond, movj_incr_msg->is_cpos);
        jogmovj->do_movj(movj_incr_msg->pos_cmd, speed, movj_param);
        break;
    }
    case ZUC_JOG_MOVL_TYPE: {
        movl_incr_msg = (ZUC_JOG_MOVL*)cmd;
        auto jogmovl = mot::al::MotionProxy::instance().robot(movl_incr_msg->robot_id);
        if (!jogmovl.get())
        {
            break;
        }
        RobTarget pos_cmd(movl_incr_msg->pos_cmd);
        MovLParam movl_param(1, movl_incr_msg->mvOpt, movl_incr_msg->tol, movl_incr_msg->executing_line_id, movl_incr_msg->end_cond);
        MoveSpeed speed(movl_incr_msg->vel, movl_incr_msg->acc, 0, movl_incr_msg->ori_vel, movl_incr_msg->ori_acc, 0);
        retval = jogmovl->do_movl(pos_cmd, speed, movl_param, 0, 0);
        break;
    }
    case ZUC_JOG_MOVC_TYPE:
        movc_msg = (ZUC_JOG_MOVC*)cmd;
        retval = zucJogMovc(movc_msg->mvOpt,
                            movc_msg->pos_mid,
                            movc_msg->pos_end,
                            movc_msg->vel,
                            movc_msg->acc,
                            movc_msg->tol,
                            movc_msg->circle_cnt,
                            movc_msg->executing_line_id,
                            movc_msg->end_cond,
                            movc_msg->circlemode);
        break;

    case ZUC_JOINT_SET_MAX_POSITION_LIMIT_TYPE:
        set_max_limit_msg = (ZUC_JOINT_SET_MAX_POSITION_LIMIT*)cmd;
        retval = zucJointSetMaxPositionLimit(set_max_limit_msg->robot_id, set_max_limit_msg->joint, set_max_limit_msg->limit);
        break;

    case ZUC_JOINT_SET_MIN_POSITION_LIMIT_TYPE: {
        set_min_limit_msg = (ZUC_JOINT_SET_MIN_POSITION_LIMIT*)cmd;
        retval = zucJointSetMinPositionLimit(set_min_limit_msg->robot_id, set_min_limit_msg->joint, set_min_limit_msg->limit);
        break;
    }

    case ZUC_TRAJ_SET_SCALE_TYPE: {
        zucTrajSetScaleMsg = (ZUC_TRAJ_SET_SCALE*)cmd;
        retval = zucTrajSetScale(zucTrajSetScaleMsg->scale);
        break;
    }
    case ZUC_TRAJ_SET_RAPID_SCALE_TYPE: {
        zucTrajSetRapidScaleMsg = (ZUC_TRAJ_SET_RAPID_SCALE*)cmd;
        zucTrajSetRapidScale(zucTrajSetRapidScaleMsg->scale);
        break;
    }
    case ZUC_TRAJ_SET_MAX_VELOCITY_TYPE: {
        break;
    }
    case ZUC_RETURN_TO_INITIAL_POSITION_TYPE:  //设置手柄回安全姿态运动速度
        // movj_incr_msg = (ZUC_JOG_MOVJ*)cmd;
        // movj_incr_msg->mvOpt = 0;
        // for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { movj_incr_msg->pos_cmd[i] = zucStatus->motion.initialPose[i]; }
        // movj_incr_msg->vel = zucStatus->motion.homeVel;
        // movj_incr_msg->acc = 90;
        // movj_incr_msg->tol = 0;
        // movj_incr_msg->executing_line_id = 0;
        // for (int i = 0; i < 3; i++) { movj_incr_msg->end_cond[i] = -1; }
        // retval = zucJogMovj(movj_incr_msg->mvOpt,
        //                     movj_incr_msg->pos_cmd.val,
        //                     movj_incr_msg->vel,
        //                     movj_incr_msg->acc,
        //                     movj_incr_msg->tol,
        //                     movj_incr_msg->executing_line_id,
        //                     movj_incr_msg->end_cond,
        //                     0);
        break;
    case ZUC_TRAJ_LINEAR_MOVE_TYPE: {
        // zucStatus->task.sleepLine = 0;
        zucTrajLinearMoveMsg = (ZUC_TRAJ_LINEAR_MOVE*)cmd;
        auto linear_move_task = mot::al::MotionProxy::instance().robot(zucTrajLinearMoveMsg->robot_id);
        if (!linear_move_task.get())
        {
            break;
        }
        RobTarget target(zucTrajLinearMoveMsg->end);
        MoveSpeed speed(zucTrajLinearMoveMsg->vel,
                        zucTrajLinearMoveMsg->acc,
                        zucTrajLinearMoveMsg->jerk,
                        zucTrajLinearMoveMsg->ori_vel,
                        zucTrajLinearMoveMsg->ori_acc,
                        0.0);
        MovLParam movl_param(0,
                             zucTrajLinearMoveMsg->type,
                             zucTrajLinearMoveMsg->ini_maxvel,
                             zucTrajLinearMoveMsg->indexrotary,
                             zucTrajLinearMoveMsg->using_abc,
                             zucTrajLinearMoveMsg->di_type,
                             zucTrajLinearMoveMsg->di_index,
                             zucTrajLinearMoveMsg->di_state,
                             zucTrajLinearMoveMsg->planner_type);
        retval = linear_move_task->do_movl(target, speed, movl_param, 0, 0);
        // retval = linear_move_task->zucTrajLinearMove(zucTrajLinearMoveMsg->end,
        //                            zucTrajLinearMoveMsg->type,
        //                            zucTrajLinearMoveMsg->vel,
        //                            zucTrajLinearMoveMsg->ini_maxvel,
        //                            zucTrajLinearMoveMsg->acc,
        //                            zucTrajLinearMoveMsg->jerk,
        //                            zucTrajLinearMoveMsg->ori_vel,
        //                            zucTrajLinearMoveMsg->ori_acc,
        //                            zucTrajLinearMoveMsg->indexrotary,
        //                            zucTrajLinearMoveMsg->using_abc,
        //                            zucTrajLinearMoveMsg->di_type,
        //                            zucTrajLinearMoveMsg->di_index,
        //                            zucTrajLinearMoveMsg->di_state,
        //                            zucTrajLinearMoveMsg->planner_type);
        // rtapi_print("sxl---------------[ZUC_TRAJ_LINEAR_MOVE_TYPE]-------------------\n");
        break;
    }

    case ZUC_TOPPRA_FINISH_LEFT_TC_TYPE: {
        toppra_finish_left_tc_msg = (ZUC_TOPPRA_FINISH_LEFT_TC*)cmd;
        auto toppra_finish_left_tc = mot::al::MotionProxy::instance().robot(0);
        if (!toppra_finish_left_tc.get())
        {
            break;
        }
        // rtapi_print(" ******************************** ZUC_TOPPRA_FINISH_LEFT_TC_TYPE ******************************** \n");
        toppra_finish_left_tc->zucToppraFinishLeftTc();
        break;
    }
    case ZUC_TOPPRA_FIRST_COMMAND_TYPE: {
        toppra_first_command_msg = (ZUC_TOPPRA_FIRST_COMMAND*)cmd;
        // rtapi_print(" ******************************** ZUC_TOPPRA_FIRST_COMMAND_TYPE ******************************** \n");
        auto toppra_first_command = mot::al::MotionProxy::instance().robot(0);
        if (!toppra_first_command.get())
        {
            break;
        }
        toppra_first_command->zucToppraFirstCommand();
        break;
    }
    case ZUC_TRAJ_CIRCULAR_MOVE_TYPE: {
        // zucStatus->task.sleepLine = 0;
        zucTrajCircularMoveMsg = (ZUC_TRAJ_CIRCULAR_MOVE*)cmd;
        auto movc_task = mot::al::MotionProxy::instance().robot(0);
        if (!movc_task.get())
        {
            break;
        }
        retval = movc_task->zucTrajCircularMove(zucTrajCircularMoveMsg->end,
                                                zucTrajCircularMoveMsg->center,
                                                zucTrajCircularMoveMsg->normal,
                                                zucTrajCircularMoveMsg->turn,
                                                zucTrajCircularMoveMsg->type,
                                                zucTrajCircularMoveMsg->vel,
                                                zucTrajCircularMoveMsg->ini_maxvel,
                                                zucTrajCircularMoveMsg->acc,
                                                zucTrajCircularMoveMsg->jerk);
        break;
    }
    case ZUC_TRAJ_JOINT_MOVE_TYPE: {
        // zucStatus->task.sleepLine = 0;
        zucTrajJointMoveMsg = (ZUC_TRAJ_JOINT_MOVE*)cmd;
        auto joint_move_task = mot::al::MotionProxy::instance().robot(zucTrajJointMoveMsg->robot_id);
        if (!joint_move_task.get())
        {
            break;
        }
        RobJointVal jpos(zucTrajJointMoveMsg->end);
        MoveSpeed speed(zucTrajJointMoveMsg->vel, zucTrajJointMoveMsg->acc, zucTrajJointMoveMsg->jerk);
        MovjParam movj_param(0,
                             zucTrajJointMoveMsg->type,
                             zucTrajJointMoveMsg->ini_maxvel,
                             zucTrajJointMoveMsg->indexrotary,
                             zucTrajJointMoveMsg->di_type,
                             zucTrajJointMoveMsg->di_index,
                             zucTrajJointMoveMsg->di_state,
                             zucTrajJointMoveMsg->jointNum,
                             zucTrajJointMoveMsg->planner_type);
        retval = joint_move_task->do_movj(jpos, speed, movj_param);
        // retval = joint_move_task->zucTrajJointMove(zucTrajJointMoveMsg->end,
        //                           zucTrajJointMoveMsg->type,
        //                           zucTrajJointMoveMsg->vel,
        //                           zucTrajJointMoveMsg->ini_maxvel,
        //                           zucTrajJointMoveMsg->acc,
        //                           zucTrajJointMoveMsg->jerk,
        //                           zucTrajJointMoveMsg->indexrotary,
        //                           zucTrajJointMoveMsg->di_type,
        //                           zucTrajJointMoveMsg->di_index,
        //                           zucTrajJointMoveMsg->di_state,
        //                           zucTrajJointMoveMsg->jointNum,
        //                           zucTrajJointMoveMsg->planner_type);
        break;
    }
    case ZUC_TRAJ_MOVS_TYPE: {
        zucTrajMovsMsg = (ZUC_TRAJ_MOVS*)cmd;
        auto movs_task = mot::al::MotionProxy::instance().robot(0);
        if (!movs_task.get())
        {
            break;
        }
        // retval = movs_task->zucTrajMovs(zucTrajMovsMsg->movs_a0, zucTrajMovsMsg->movs_a1, zucTrajMovsMsg->movs_a2, zucTrajMovsMsg->movs_a3, zucTrajMovsMsg->movs_time);
        Movs_Param movs_param(zucTrajMovsMsg->movs_a0, zucTrajMovsMsg->movs_a1, zucTrajMovsMsg->movs_a2, zucTrajMovsMsg->movs_a3, zucTrajMovsMsg->movs_time);
        retval = movs_task->do_movs(movs_param);
        break;
    }
    case ZUC_TRAJ_MOVC_TYPE: {
        // zucStatus->task.sleepLine = 0;
        zucTrajMovcMsg = (ZUC_TRAJ_MOVC*)cmd;
        auto movc_task = mot::al::MotionProxy::instance().robot(0);
        if (!movc_task.get())
        {
            break;
        }
        //retval = zucTrajJointMove(zucTrajJointMoveMsg->end, zucTrajJointMoveMsg->type, zucTrajJointMoveMsg->vel, zucTrajJointMoveMsg->ini_maxvel,
        //		zucTrajJointMoveMsg->acc, zucTrajJointMoveMsg->jerk, zucTrajJointMoveMsg->indexrotary);
        RobTarget target(zucTrajMovcMsg->endPoint);
        RobTarget mid(zucTrajMovcMsg->midPoint);
        MoveSpeed speed(zucTrajMovcMsg->vel, zucTrajMovcMsg->acc, zucTrajMovcMsg->jerk, 0, 0, 0);
        Movc_Param movc_param(0,
                              zucTrajMovcMsg->ini_maxvel,
                              zucTrajMovcMsg->using_abc,
                              zucTrajMovcMsg->di_type,
                              zucTrajMovcMsg->di_index,
                              zucTrajMovcMsg->di_state,
                              zucTrajMovcMsg->planner_type,
                              zucTrajMovcMsg->circle_count,
                              zucTrajMovcMsg->circlemode);
        retval = movc_task->do_movc(target, mid, speed, movc_param, 0, 0);
        // retval = movc_task->zucTrajMovc(zucTrajMovcMsg->midPoint,
        //                      zucTrajMovcMsg->endPoint,
        //                      zucTrajMovcMsg->vel,
        //                      zucTrajMovcMsg->ini_maxvel,
        //                      zucTrajMovcMsg->acc,
        //                      zucTrajMovcMsg->jerk,
        //                      zucTrajMovcMsg->using_abc,
        //                      zucTrajMovcMsg->circle_count,
        //                      zucTrajMovcMsg->di_type,
        //                      zucTrajMovcMsg->di_index,
        //                      zucTrajMovcMsg->di_state,
        //                      zucTrajMovcMsg->planner_type,
        //                      zucTrajMovcMsg->circlemode);
        break;
    }
    case ZUC_TRAJ_PAUSE_TYPE: {
        if (zucStatus->isSteppingMode && zucStatus->steppingState == ZUC_TASK_INTERP_READING)
        {
            zucStatus->steppingState = ZUC_TASK_INTERP_PAUSED;
        }
        zucStatus->task.task_paused = 1;
        retval = zucTrajPause(-1);  // 默认停止所有机器人
        break;
    }
    case ZUC_TRAJ_RESUME_TYPE: {
        retval = 0;
        if (zucStatus->task.task_paused)
        {
            zucStatus->task.task_paused = 0;
            retval = zucTrajResume(-1);  // 默认恢复所有机器人
        }
        break;
    }
    case ZUC_TRAJ_ABORT_TYPE: {
        auto abort_type = mot::al::MotionProxy::instance().robot(0);
        if (!abort_type.get())
        {
            break;
        }
        retval = abort_type->do_stop();
        break;
    }
    case ZUC_TRAJ_DELAY_TYPE:
        zucTrajDelayMsg = (ZUC_TRAJ_DELAY*)cmd;
        // set the timeout clock to expire at 'now' + delay time
        taskExecDelayTimeout = etime() + zucTrajDelayMsg->delay;
        zucStatus->task.sleepLine = zucTrajDelayMsg->line_num;
        retval = 0;
        break;

    case ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET_TYPE: {
        // update robot tool offset
        auto zucTrajSetRobotToolOffsetMsg = (ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET*)cmd;
        // retval = zucTrajSetRobotToolOffset(zucTrajSetRobotToolOffsetMsg->robot_id, zucTrajSetRobotToolOffsetMsg->robotToolOffset);
        break;
    }

    case ZUC_TRAJ_SET_ROBOT_USER_FRAME_TYPE: {
        auto set_robot_user_frame = mot::al::MotionProxy::instance().robot(0);
        if (!set_robot_user_frame.get())
        {
            break;
        }
        // update robot tool offset
        zucTrajSetRobotUserFrmsetMsg = (ZUC_TRAJ_SET_ROBOT_USER_FRAME*)cmd;
        zucStatus->task.robotUserOffset = zucTrajSetRobotUserFrmsetMsg->robotUserFrame;
        retval = zucTrajSetRobotUserOffset(zucTrajSetRobotUserFrmsetMsg->robot_id, zucTrajSetRobotUserFrmsetMsg->robotUserFrame);
        break;
    }

    case ZUC_TRAJ_SET_PAYLOAD_TYPE: {
        zucTrajSetPayloadMsg = (ZUC_TRAJ_SET_PAYLOAD*)cmd;
        retval = zucTrajSetPayload(zucTrajSetPayloadMsg->id, zucTrajSetPayloadMsg->payload);
        break;
    }
    case ZUC_TRAJ_CALIB_DHPARAM_TYPE:
        zucTrajCalibDHParamMsg = (ZUC_TRAJ_CALIB_DHPARAM*)cmd;
        retval = zucCalibDHParams(zucTrajCalibDHParamMsg->dhParamError);
        break;

    case ZUC_AUX_INPUT_WAIT_TYPE:
        zucAuxInputWaitMsg = (ZUC_AUX_INPUT_WAIT*)cmd;
        auxInputWaitMsg(zucAuxInputWaitMsg);
        break;

    case ZUC_TRAJ_RIGID_TAP_TYPE: {
        auto traj_rigid_tap = mot::al::MotionProxy::instance().robot(0);
        if (!traj_rigid_tap.get())
        {
            break;
        }
        retval = traj_rigid_tap->zucTrajRigidTap(
            ((ZUC_TRAJ_RIGID_TAP*)cmd)->pos, ((ZUC_TRAJ_RIGID_TAP*)cmd)->vel, ((ZUC_TRAJ_RIGID_TAP*)cmd)->ini_maxvel, ((ZUC_TRAJ_RIGID_TAP*)cmd)->acc);
        break;
    }
    case ZUC_TRAJ_SET_TELEOP_ENABLE_TYPE: {
        break;
    }
    case ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE_TYPE: {
        break;
    }
    case ZUC_TRAJ_SET_SERVOJOP_ENABLE_TYPE: {
        auto task_cmd = (ZUC_TRAJ_SET_SERVOJOP_ENABLE*)cmd;
        zucSetServoEnable(task_cmd->robot_id, task_cmd->enable);
        break;
    }
    case ZUC_TRAJ_SET_ADMITOP_ENABLE_TYPE: {
        auto set_admitop_enable_type = mot::al::MotionProxy::instance().robot(0);
        if (!set_admitop_enable_type.get())
        {
            break;
        }
        if (((ZUC_TRAJ_SET_ADMITOP_ENABLE*)cmd)->enable)
        {
            // set task no delay
            // if(zucStatus->motion.sensor.sensorStatus.status != 1) {
            // cannot enable admittance control since sensor is not ready
            // 	zucOperatorError(ADMIT_CANNOT_ENABLE_WITHOUT_SENSOR, _("Cannot enable admittance control since sensor is not ready"));
            // }
            retval = set_admitop_enable_type->zucTrajSetMode(ZUC_TRAJ_MODE_ADMITTANCE);
        }
        else
        {
            retval = set_admitop_enable_type->zucTrajSetMode(ZUC_TRAJ_MODE_FREE);
        }
        break;
    }
    case ZUC_TRAJ_SET_ADMITTANCE_ENABLE_TYPE: {
        ZUC_TRAJ_SET_ADMITTANCE_ENABLE* setAdmitEnaCmd = (ZUC_TRAJ_SET_ADMITTANCE_ENABLE*)cmd;
        if (setAdmitEnaCmd->enable && zucStatus->motion.sensor.sensorStatus.status != 1)
        {
            zucOperatorError(ADMIT_CANNOT_ENABLE_WITHOUT_SENSOR, _("Cannot enable admittance control since sensor is not ready"));
            retval = -1;
        }
        else
        {
            retval = zucTrajSetAdmittanceEnable(setAdmitEnaCmd->enable);
        }
        break;
    }

    case ZUC_TRAJ_SET_COMPLIANCE_ENABLE_TYPE: {
        ZUC_TRAJ_SET_COMPLIANCE_ENABLE* setAdmitEnaCmd = (ZUC_TRAJ_SET_COMPLIANCE_ENABLE*)cmd;
        //rcs_print("setAdmitEnaCmd: %d, %d\n", setAdmitEnaCmd->compliantType, setAdmitEnaCmd->compliantEnable);
        //retval = zucTrajSetComplianceInit(setAdmitEnaCmd->robot_id, setAdmitEnaCmd->compliantType,setAdmitEnaCmd->compliantEnable,1);
        if (setAdmitEnaCmd->compliantType && zucStatus->motion.sensor.sensorStatus.status != 1)
        {
            zucOperatorError(ADMIT_CANNOT_ENABLE_WITHOUT_SENSOR, _("Cannot enable admittance control since sensor is not ready"));
            retval = -1;
        }
        else
        {
            retval = zucTrajSetComplianceInit(setAdmitEnaCmd->robot_id, setAdmitEnaCmd->compliantType, setAdmitEnaCmd->compliantEnable);
        }
        break;
    }

    case ZUC_TRAJ_SET_ADMITTANCE_FRAME_TYPE: {
        ZUC_TRAJ_SET_ADMITTANCE_FRAME* setAdmitEnaCmd = (ZUC_TRAJ_SET_ADMITTANCE_FRAME*)cmd;
        retval = zucTrajSetAdmittanceCtrlFrame(setAdmitEnaCmd->robot_id, setAdmitEnaCmd->ftFrame);
        break;
    }

    case ZUC_TRAJ_DISABLE_FORCE_CONTROL_TYPE: {
        ZUC_TRAJ_DISABLE_FORCE_CONTROL* setAdmitEnaCmd = (ZUC_TRAJ_DISABLE_FORCE_CONTROL*)cmd;
        //rcs_print(" setAdmitEnaCmd->disableForceCtrl: %d \n", setAdmitEnaCmd->disableForceCtrl);
        retval = zucTrajDisableForceControl(setAdmitEnaCmd->disableForceCtrl);
        break;
    }

    case ZUC_TRAJ_SET_ADMITTANCE_OPTION_TYPE: {
        ZUC_TRAJ_SET_ADMITTANCE_OPTION* setAdmitOptCmd = (ZUC_TRAJ_SET_ADMITTANCE_OPTION*)cmd;
        retval = zucTrajSetAdmittanceOption(setAdmitOptCmd->admit_axis_opt);
        break;
    }

    case ZUC_TRAJ_SET_ADMITTANCE_CONFIG_TYPE: {
        ZUC_TRAJ_SET_ADMITTANCE_CONFIG* setAdmitConCmd = (ZUC_TRAJ_SET_ADMITTANCE_CONFIG*)cmd;
        retval = zucTrajSetAdmittanceConfig(setAdmitConCmd->robot_id, setAdmitConCmd->axis, setAdmitConCmd->ftConfig, setAdmitConCmd->now);
        break;
    }

    case ZUC_TRAJ_SET_END_FORCE_CONDITION_TYPE: {
        ZUC_TRAJ_SET_END_FORCE_CONDITION* setEndForceConCmd = (ZUC_TRAJ_SET_END_FORCE_CONDITION*)cmd;
        retval = zucTrajSetEndForceCond(setEndForceConCmd->axis, setEndForceConCmd->endForceCond);
        break;
    }
    case ZUC_SET_SAFE_ZONE_TYPE: {
        ZUC_SET_SAFE_ZONE* setSafeZone = (ZUC_SET_SAFE_ZONE*)cmd;
        // rcs_print("setSafeZone->openEnable=%d, setSafeZone->autoEnable=%d, setSafeZone->safeZoneMode=%d\n",
        //           setSafeZone->openEnable,
        //           setSafeZone->autoEnable,
        //           setSafeZone->zone.safeZoneMode);

        xmlSaveSafeZone(setSafeZone->zone,
                        setSafeZone->openEnable,
                        setSafeZone->autoEnable,
                        zucStatus->userOffset[zucStatus->motion.currentUserId],
                        zucStatus->safe.safetyZone,
                        setSafeZone->coordinateSystemTransformation);
        break;
    }

    case ZUC_SET_PLANNER_JERK_TYPE: {
        auto set_planner_jerk = mot::al::MotionProxy::instance().robot(0);
        if (!set_planner_jerk.get())
        {
            break;
        }
        ZUC_SET_PLANNER_JERK* setplannerjerk = (ZUC_SET_PLANNER_JERK*)cmd;
        //rcs_print("setSafeZone->openEnable=%d, setSafeZone->autoEnable=%d, safe=%d\n", setSafeZone->openEnable, setSafeZone->autoEnable, setSafeZone->zone.safe);
        retval = zucTrajSetPlannerJerk(setplannerjerk->robot_id, setplannerjerk->joint_jerk, setplannerjerk->carte_jerk);
        break;
    }

    case ZUC_SET_SAFE_ATTITUDE_TYPE: {
        ZUC_SET_SAFE_ATTITUDE* setSafeAttitude = (ZUC_SET_SAFE_ATTITUDE*)cmd;
        //rcs_print("setSafeZone->openEnable=%d, setSafeZone->autoEnable=%d, safe=%d\n", setSafeZone->openEnable, setSafeZone->autoEnable, setSafeZone->zone.safe);
        retval = zucTrajSetSafeAttitude(setSafeAttitude->conicalRpy,
                                        setSafeAttitude->tilt,
                                        setSafeAttitude->pan,
                                        setSafeAttitude->deviation,
                                        setSafeAttitude->deviationwarn,
                                        setSafeAttitude->openEnable,
                                        setSafeAttitude->autoEnable,
                                        setSafeAttitude->tipChoice,
                                        1);
        break;
    }

    case ZUC_ROBOT_SAFETY_TYPE:
        set_robot_safety = (ZUC_ROBOT_SAFETY*)cmd;
        if (set_robot_safety->type == 0)
        {
            retval = zucSetRobotSafety(&zucStatus->motion, *set_robot_safety);
        }
        else if (set_robot_safety->type == 1)
        {
            retval = zucGetRobotSafety(&zucStatus->motion);
        }
        break;

    case Robot_7_DOF_INV_INFORM_TYPE: {
        Robot_7_DOF_INV_INFORM* robot_7_dof_inv = (Robot_7_DOF_INV_INFORM*)cmd;
        retval = SetRobot7dofinv_inform(robot_7_dof_inv->inv_flag, robot_7_dof_inv->arm_angle);
        break;
    }

    case Robot_SET_FULL_DH_FLAG_TYPE: {
        Robot_SET_FULL_DH_FLAG* set_full_dh = (Robot_SET_FULL_DH_FLAG*)cmd;
        retval = SetRobotFull_DH_flag(set_full_dh->full_dh_flag);
    }

    case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE: {
        Robot_SET_7_DOF_KINE_PARAM_ALPHA_K* robot_7_dof_param = (Robot_SET_7_DOF_KINE_PARAM_ALPHA_K*)cmd;
        retval = SetRobot7dof_kine_param_alpha_k(robot_7_dof_param->k, robot_7_dof_param->alpha);
    }

    case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE: {
        ZUC_SET_ROBOT_SAFETY_CUSTOMIZE* robotSafetyCustomize = (ZUC_SET_ROBOT_SAFETY_CUSTOMIZE*)cmd;
        zucSetCustomizeType(robotSafetyCustomize->customize_type);
        break;
    }

    case ZUC_REDUCE_SAFETY_TYPE: {
        set_reduce_safety = (ZUC_REDUCE_SAFETY*)cmd;
        auto reduce_safety = mot::al::MotionProxy::instance().robot(0);
        if (!reduce_safety.get())
        {
            break;
        }
        reduce_param_config reduce_param;
        reduce_param.tcp_vel_reduce = set_reduce_safety->tcp_speed;
        reduce_param.elbow_vel_limit_reduce = set_reduce_safety->elbow_speed;
        reduce_param.robot_momentum_limit_reduce = set_reduce_safety->momentum_limit;
        reduce_param.robot_poewr_limit_reduce = set_reduce_safety->power_limit;
        reduce_param.stopping_dist_reduce = set_reduce_safety->stopping_dist;
        reduce_param.stopping_time_reduce = set_reduce_safety->stopping_time;
        retval = reduce_safety->zucSetReduceSafety(&zucStatus->motion, reduce_param);
        break;
    }
    case ZUC_DRAG_SPEED_LIMIT_TYPE: {
        ZUC_DRAG_SPEED_LIMIT* drag_limit = (ZUC_DRAG_SPEED_LIMIT*)cmd;
        auto drag_speed_limit = mot::al::MotionProxy::instance().robot(0);
        if (!drag_speed_limit.get())
        {
            break;
        }
        retval = drag_speed_limit->zucSetDragTcpSpeedLimitSetting(drag_limit->drag_tcp_speed_limit);
        break;
    }
    case ZUC_STEP_STATUS_TYPE:
        step_status = (ZUC_STEP_STATUS*)cmd;
        zucStatus->stepStatus.isSteping = 0;
        zucStatus->stepStatus.programRunTrigger = 0;
        zucStatus->stepStatus.stepEnd = 0;
        break;

    case ZUC_JOINT_SAFETY_TYPE: {
        set_joint_safety = (ZUC_JOINT_SAFETY*)cmd;
        if (set_joint_safety->type == 0)
        {
            retval = zucSetJointSafety(set_joint_safety->robot_id,
                                       set_joint_safety->jointNum,
                                       set_joint_safety->paramType,
                                       set_joint_safety->jointVelLimit,
                                       set_joint_safety->jointErrLimit);
        }
        else if (set_joint_safety->type == 1)
        {
            retval = zucGetJointSafety(&zucStatus->motion);  // 无意义
        }
        break;
    }

    case ZUC_MOTION_SET_AOUT_TYPE:
        if (((ZUC_MOTION_SET_AOUT*)cmd)->type == 2 && zucStatus->io.extio.extIOStat.status != 1)
        {  // extio
            zucOperatorError(CANNOT_ACCESS_EXTIO_SINCE_OFFLINE, _("cannot access extio since it's not running"));
            zucTaskAbort();
            zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
            retval = -1;
        }
        else
        {
            retval = zucMotionSetAout(((ZUC_MOTION_SET_AOUT*)cmd)->type,
                                      ((ZUC_MOTION_SET_AOUT*)cmd)->index,
                                      ((ZUC_MOTION_SET_AOUT*)cmd)->start,
                                      ((ZUC_MOTION_SET_AOUT*)cmd)->end,
                                      ((ZUC_MOTION_SET_AOUT*)cmd)->now);
        }
        break;

    case ZUC_MOTION_SET_DOUT_TYPE: {
        ZUC_MOTION_SET_DOUT* cmd_ptr = (ZUC_MOTION_SET_DOUT*)cmd;
        if (cmd_ptr->type < 0 || cmd_ptr->type > 6)
        {
            printf("set_Dout():  UNKOWN  IO Type  %d \n", cmd_ptr->type);
            break;
        }
        printf("SET_DOUT:  %d,  %d\n", cmd_ptr->type, cmd_ptr->index);
        bool fd_isset = FD_ISSET(cmd_ptr->index, &zucStatus->io.funcIOMap.digtalOutput_bound[cmd_ptr->type]);
        if (cmd_ptr->type == 4)
        {  // io 重构后 modbus 的iotype=4,但是在 func IO配置时使用的还是type 0.
            int index_offset = 0;
            if (zucStatus->cab_type == 2 || zucStatus->cab_type == 4)  // cabV2.1  or  cabV3
            {
                index_offset = ZUCMOT_SCB_USER_DIO_NUM;
            }
            else if (zucStatus->cab_type == 3)  // mini Cab
            {
                index_offset = ZUCMOT_MINI_USER_DIO_NUM;
            }
            fd_isset |= FD_ISSET(cmd_ptr->index + index_offset, &zucStatus->io.funcIOMap.digtalOutput_bound[0]);
        }
        if (fd_isset)
        {
            char str[256];
            char ioType_str[7][32] = {"Cabinet_DO", "TooL_DO", "Extended_DO", "   ", "Modbus_DO", "Profinet_DO", "EIP_DO"};
            snprintf(str,
                     256,
                     "{\"fixed_key\":[\"%s\",\"%d\",\"%s\",\"%d\",\"%s\",\"%d\"]}",
                     ioType_str[cmd_ptr->type],
                     cmd_ptr->index,
                     ioType_str[cmd_ptr->type],
                     cmd_ptr->index,
                     ioType_str[cmd_ptr->type],
                     cmd_ptr->index);
            zucOperatorError(
                WARNING_DOUT_PORT_UNAVAILABLE, "digital output [%d:%d]  is bound to a special function  # %s ", cmd_ptr->type, cmd_ptr->index, str);
            printf("digital output [%s:%d]  is bound to a special function \n\n ", ioType_str[cmd_ptr->type], cmd_ptr->index);
            // 报错但不停止程序。（新版APP 在运行程序时会提示异常，程序不会运行。SDK 或者Addon运行指令仅报错，不终止主程序。 ）
            // zucTaskAbort();
            // zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
            retval = -1;
            break;
        }
        if (((ZUC_MOTION_SET_DOUT*)cmd)->type == 2 && zucStatus->io.extio.extIOStat.status != 1)
        {  // extio
            zucOperatorError(CANNOT_ACCESS_EXTIO_SINCE_OFFLINE, _("cannot access extio since it's not running"));
            zucTaskAbort();
            zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
            retval = -1;
        }
        else
        {
            //
            retval = zucMotionSetDout(((ZUC_MOTION_SET_DOUT*)cmd)->type,
                                      ((ZUC_MOTION_SET_DOUT*)cmd)->index,
                                      ((ZUC_MOTION_SET_DOUT*)cmd)->start,
                                      ((ZUC_MOTION_SET_DOUT*)cmd)->end,
                                      ((ZUC_MOTION_SET_DOUT*)cmd)->now);
        }
        break;
    }

    case ZUC_MOTION_ADAPTIVE_TYPE: {
        auto motion_adaptive = mot::al::MotionProxy::instance().robot(0);
        if (!motion_adaptive.get())
        {
            break;
        }
        retval = motion_adaptive->zucTrajSetAFEnable(((ZUC_MOTION_ADAPTIVE*)cmd)->status);
        break;
    }
    case ZUC_MOTION_EXIT_PROTECT_TYPE: {
        auto motion_exit_protect = mot::al::MotionProxy::instance().robot(0);
        if (!motion_exit_protect.get())
        {
            break;
        }
        retval = motion_exit_protect->zucMotionExitProtect(((ZUC_MOTION_EXIT_PROTECT*)cmd)->protect_type);
        break;
    }
    case ZUC_MOTION_PERCENTAGE_MODE_TYPE: {
        auto motion_percentage_mode = mot::al::MotionProxy::instance().robot(0);
        if (!motion_percentage_mode.get())
        {
            break;
        }
        retval = motion_percentage_mode->zucMotionPercentageMode(((ZUC_MOTION_PERCENTAGE_MODE*)cmd)->percentage_level);
        break;
    }
    case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE: {
        auto percentage_mode_level_value = mot::al::MotionProxy::instance().robot(0);
        if (!percentage_mode_level_value.get())
        {
            break;
        }
        retval = percentage_mode_level_value->zucMotionPercentageModeLevelValue(((ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE*)cmd)->percentage_mode_level1_val,
                                                                                ((ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE*)cmd)->percentage_mode_level2_val);
        break;
    }
    case ZUC_MOTION_REDUCE_MODE_TYPE: {
        auto reduce_mode = mot::al::MotionProxy::instance().robot(0);
        if (!reduce_mode.get())
        {
            break;
        }
        retval = reduce_mode->zucMotReduceMode(((ZUC_MOTION_REDUCE_MODE*)cmd)->reduce_mode);
        break;
    }
    case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
        // retval = zucMotionFreeButtonOpt(robot_id, ((ZUC_MOTION_SET_FREE_BUTTON_OPT*)cmd)->freeButton);
        break;

    case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE: {
        // retval = zucMotionPointButtonOpt(robot_id, ((ZUC_MOTION_SET_POINT_BUTTON_OPT*)cmd)->pointButton);
        break;
    }

    case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE: {
        ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT* optCmd = (ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT*)cmd;
        // retval = zucMotionFreeProgButtonOpt(optCmd->robot_id, optCmd->freeButton, optCmd->progButton);
        break;
    }

    case ZUC_MOTION_KINE_INVERSE_TYPE: {
        ZUC_MOTION_KINE_INVERSE* invCmd = (ZUC_MOTION_KINE_INVERSE*)cmd;
        retval = zucMotionKineInverse(invCmd->robindex, &(invCmd->tarPose), invCmd->currJoint);
        break;
    }

    case ZUC_MOTION_KINE_FORWARD_TYPE: {
        ZUC_MOTION_KINE_FORWARD* fwdCmd = (ZUC_MOTION_KINE_FORWARD*)cmd;
        retval = zucMotionKineForward(fwdCmd->robindex, fwdCmd->joints);
        break;
    }
    //
    case ZUC_MOTION_SET_FUNCDI_TYPE: {
        ZUC_MOTION_SET_FUNCDI* funcIOMAPCmd = (ZUC_MOTION_SET_FUNCDI*)cmd;
        retval = zucSetFuncDIMap(funcIOMAPCmd->funcIOMap, 1);
        break;
    }

    case ZUC_MOTION_SET_FUNCDO_TYPE: {
        ZUC_MOTION_SET_FUNCDO* funcIOMAPCmd = (ZUC_MOTION_SET_FUNCDO*)cmd;
        retval = zucSetFuncDOMap(funcIOMAPCmd->funcIOMap, 1);
        break;
    }

    case ZUC_MOTION_SET_SAFETY_FUNCDI_TYPE: {
        ZUC_MOTION_SET_SAFETY_FUNCDI* funcIOMAPCmd = (ZUC_MOTION_SET_SAFETY_FUNCDI*)cmd;
        retval = zucSetSafetyFuncDIMap(funcIOMAPCmd->sFuncDISettings, 1);
        break;
    }

    case ZUC_MOTION_SET_SAFETY_FUNCDO_TYPE: {
        ZUC_MOTION_SET_SAFETY_FUNCDO* funcIOMAPCmd = (ZUC_MOTION_SET_SAFETY_FUNCDO*)cmd;
        retval = zucSetSafetyFuncDOMap(funcIOMAPCmd->sFuncDOSettings, 1);
        break;
    }

    case ZUC_MOTION_SET_HOME_VEL_TYPE: {
        ZUC_MOTION_SET_HOME_VEL* setHomeVel = (ZUC_MOTION_SET_HOME_VEL*)cmd;
        if (setHomeVel->setFlag == 1)
        {
            zucSetHomeVel(setHomeVel->robot_id, setHomeVel->homeVel);
            setHomeVel->setFlag = 0;
        }
        break;
    }
    case ZUC_SET_SERVO_PARAM_TYPE: {
        ZUC_SET_SERVO_PARAM* setServoParamCmd = (ZUC_SET_SERVO_PARAM*)cmd;
        retval = zucSetServoParam(setServoParamCmd->robot_id, setServoParamCmd->servoParam);
        break;
    }

    case ZUC_MOTION_ONE_KEY_OP_TYPE: {
        ZUC_MOTION_ONE_KEY_OP* oneKeyCmd = (ZUC_MOTION_ONE_KEY_OP*)cmd;
        // retval = zucOneKeyOp(oneKeyCmd->robot_id, oneKeyCmd->onekeyenable, oneKeyCmd->autoprogram);
        break;
    }

    case ZUC_MOTION_SET_CAPTURE_TYPE: {
        ZUC_MOTION_SET_CAPTURE* captCmd = (ZUC_MOTION_SET_CAPTURE*)cmd;
        retval = zucSetCapture(captCmd->captureId, captCmd->trigger, captCmd->stateToCapt);
        break;
    }

    case ZUC_MOTION_CLEAR_CAPTURE_TYPE: {
        ZUC_MOTION_CLEAR_CAPTURE* clrCmd = (ZUC_MOTION_CLEAR_CAPTURE*)cmd;
        retval = zucClearCapture(clrCmd->captureId);
        break;
    }

    case ZUC_SHUT_DOWN_TYPE:
        if (zucStatus->motion.powered_on)
        {
            zucOperatorError(
                CANNOT_EXECUTE_WHILE_ROBOT_POWERED_ON, "please power off first before SHUT-DOWN # {\"fixed_key\":[\"%s\",\"%s\"]}", "SHUT-DOWN", "SHUT-DOWN");
            retval = -1;
            break;
        }
        if (zucStatus->motion.is_enabled)
        {
            zucOperatorError(SERVO_OFF_BEFORE_POWER_OFF, "%s", "please servo off first before powering off the robot");
            retval = -1;
            break;
        }
        zucShutDown();
        zuctask_shutdown();
        esleep(1);
        sync();
        sync();
        sync();
        reboot(RB_POWER_OFF);
        exit(1);
        break;

    case ZUC_MOTION_SETUP_IO_CHANNEL_TYPE: {
        ZUC_MOTION_SETUP_IO_CHANNEL* setupAnaChnCmd = (ZUC_MOTION_SETUP_IO_CHANNEL*)cmd;
        // retval = zucSetupIOChannel(setupAnaChnCmd->robot_id, setupAnaChnCmd->channel_type, setupAnaChnCmd->channel_index, setupAnaChnCmd->channel_setting);
        break;
    }

    case ZUC_MOTION_SET_BR_STRT_VOLTAGE_TYPE: {
        if (!zucStatus->motion.powered_on)
        {
            ZUC_MOTION_SET_BR_STRT_VOLTAGE* setScbBrStrtVol = (ZUC_MOTION_SET_BR_STRT_VOLTAGE*)cmd;
            // retval = zucSetBRStartVoltage(setScbBrStrtVol->robot_id, setScbBrStrtVol->starting_voltage);
        }
        else
        {
            zucOperatorError(OPERATION_IN_WRONG_MODE,
                             "Cannot set starting voltage for braking resistor with power on#{\"fixed_key\":[\"POWERON\",\"SET_BR_STRT_VOLTAGE\"]}");
            retval = -1;
        }
        break;
    }

    case ZUC_SERVO_UPGRADE_TYPE: {
        retval = zucServoUpGrade();
        break;
    }
    case ZUC_SET_CABVOLTAGE_TYPE: {
        int vol = ((ZUC_SET_CABVOLTAGE*)cmd)->cabVoltage;
        if (vol > 0)
            zucSetCabVoltage((unsigned int)vol);
        break;
    }
    case ZUC_SET_DEBUG_TYPE: {
        auto set_debug = mot::al::MotionProxy::instance().robot(0);
        if (!set_debug.get())
        {
            break;
        }
        zuc_debug = ((ZUC_SET_DEBUG*)cmd)->debug;
        set_debug->zucMotionSetDebug(zuc_debug);
        zucStatus->debug = zuc_debug;
        break;
    }
    case ZUC_APP_SET_CONNECTSTAT_TYPE: {
        zucStatus->appConnectionStatus = (((ZUC_APP_SET_CONNECTSTAT*)cmd)->status) == 0 ? false : true;
        break;
    }

    case ZUC_EXTIO_SETUP_TYPE: {
        ZUC_EXTIO_SETUP* extIOSetupCmd = (ZUC_EXTIO_SETUP*)cmd;
        zucExtIOSetup(extIOSetupCmd->extIOModCnt, extIOSetupCmd->extIOSetup, 1);  //
        break;
    }

    case ZUC_SETUP_MODBUS_SLAVE_TYPE: {
        ZUC_SETUP_MODBUS_SLAVE* setModbusSlave = (ZUC_SETUP_MODBUS_SLAVE*)cmd;
        zucModbusSetupSlave(setModbusSlave->commType, setModbusSlave->mbComm);
        break;
    }

    case ZUC_EXTIO_SETMODE_TYPE: {
        ZUC_EXTIO_SETMODE* extIOSetModeCmd = (ZUC_EXTIO_SETMODE*)cmd;
        zucExtIOSetMode(extIOSetModeCmd->mode);
        break;
    }
    case ZUC_SET_MAINTENANCE_MODE_TYPE: {
        ZUC_SET_MAINTENANCE_MODE* maintenanceMode = (ZUC_SET_MAINTENANCE_MODE*)cmd;
        zucSetMaintenanceMode(maintenanceMode->maintenance);
        break;
    }

    case ZUC_TORQSENSOR_SETCOMM_TYPE: {
        ZUC_TORQSENSOR_SETCOMM* sensorSetModeCmd = (ZUC_TORQSENSOR_SETCOMM*)cmd;
        // zucTorqSensorSetComm(sensorSetModeCmd->robot_id, sensorSetModeCmd->commType, sensorSetModeCmd->commCfg);
        break;
    }

    case ZUC_TORQSENSOR_SETPAYLOAD_TYPE: {
        ZUC_TORQSENSOR_SETPAYLOAD* sensorSetPayloadCmd = (ZUC_TORQSENSOR_SETPAYLOAD*)cmd;
        // zucTorqSensorSetPayload(sensorSetPayloadCmd->robot_id, sensorSetPayloadCmd->payload);
        break;
    }

    case ZUC_TOOL_SETPAYLOAD_TYPE: {
        ZUC_TOOL_SETPAYLOAD* toolSetPayloadCmd = (ZUC_TOOL_SETPAYLOAD*)cmd;
        // zucToolSetPayload(toolSetPayloadCmd->robot_id, toolSetPayloadCmd->payload);
        break;
    }

    case ZUC_TORQUE_SENSOR_SOFT_LIMIT_TYPE: {
        ZUC_TORQUE_SENSOR_SOFT_LIMIT* torqueSensorSoftLimit = (ZUC_TORQUE_SENSOR_SOFT_LIMIT*)cmd;
        double limit[6] = {torqueSensorSoftLimit->Fx,
                           torqueSensorSoftLimit->Fy,
                           torqueSensorSoftLimit->Fz,
                           torqueSensorSoftLimit->Mx,
                           torqueSensorSoftLimit->My,
                           torqueSensorSoftLimit->Mz};
        zucTorqueSensorSoftLimit(torqueSensorSoftLimit->robot_id, limit);
        break;
    }

    case ZUC_FUNCTION_SAFTY_PARA_TYPE: {
        ZUC_FUNCTION_SAFTY_PARA* task_cmd = (ZUC_FUNCTION_SAFTY_PARA*)cmd;
        zucSetFunctionSaftyPara(task_cmd->robot_power_limit, task_cmd->robot_momentum_limit, task_cmd->robot_endforce_limit, task_cmd->robot_velocity_limit);
        break;
    }

    case ZUC_ADMITTANCE_DRAG_DEAD_ZONE_TYPE: {
        ZUC_ADMITTANCE_DRAG_DEAD_ZONE* dead_zone = (ZUC_ADMITTANCE_DRAG_DEAD_ZONE*)cmd;
        double drag_dead_zone[CART_DIM] = {dead_zone->Fx, dead_zone->Fy, dead_zone->Fz, dead_zone->Mx, dead_zone->My, dead_zone->Mz};
        zucAdmittanceDragDeadZone(dead_zone->robot_id, drag_dead_zone);
        break;
    }

    case ZUC_TORQUE_SENSOR_FILTER_TYPE: {
        ZUC_TORQUE_SENSOR_FILTER* torqueSensorFilter = (ZUC_TORQUE_SENSOR_FILTER*)cmd;
        zucTorqueSensorFilter(torqueSensorFilter->robot_id, torqueSensorFilter->torqueSensorFilter);
        break;
    }

    case ZUC_TORQSENSOR_SETMODE_TYPE: {
        ZUC_TORQSENSOR_SETMODE* sensorSetModeCmd = (ZUC_TORQSENSOR_SETMODE*)cmd;
        if (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_ADMITTANCE)
        {
            // zucTorqSensorSetMode(sensorSetModeCmd->robot_id, sensorSetModeCmd->mode);
        }
        else
        {
            // cannot switch sensor mode during admittance control
            zucOperatorError(ADMIT_CANNOT_SET_MODE_IN_OPERATION, _("Cannot switch senor mode when in admittance control"));
        }
        break;
    }

    case ZUC_TORQSENSOR_SETBRAND_TYPE: {
        ZUC_TORQSENSOR_SETBRAND* sensorSetBrandCmd = (ZUC_TORQSENSOR_SETBRAND*)cmd;
        if (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_ADMITTANCE)
        {
            // zucTorqSensorSetBrand(sensorSetBrandCmd->robot_id, sensorSetBrandCmd->torqsensorBrand);
        }
        else
        {
            // cannot switch sensor brand when the robot is in  admittance control
            zucOperatorError(ADMIT_CANNOT_SET_MODE_IN_OPERATION, _("Cannot switch senor brand when the robot is in admittance control"));
        }
        break;
    }

    case ZUC_MBTIO_ADD_SIGNAL_TYPE: {
        ZUC_MBTIO_ADD_SIGNAL* addSigCmd = (ZUC_MBTIO_ADD_SIGNAL*)cmd;
        // zucTioMBAddSignal(addSigCmd->robot_id, addSigCmd->sigName, addSigCmd->chnId, addSigCmd->type, addSigCmd->addr);
        break;
    }

    case ZUC_MBTIO_DEL_SIGNAL_TYPE: {
        ZUC_MBTIO_DEL_SIGNAL* delSigCmd = (ZUC_MBTIO_DEL_SIGNAL*)cmd;
        zucTioMBDelSignal(delSigCmd->sigName);
        break;
    }

    case ZUC_MBTIO_UPDATE_SIGNAL_TYPE: {
        ZUC_MBTIO_UPDATE_SIGNAL* updateSigCmd = (ZUC_MBTIO_UPDATE_SIGNAL*)cmd;
        zucTioMBUpdateSignal(updateSigCmd->sigName, updateSigCmd->freq);
        break;
    }

    case ZUC_MBTIO_SEND_COMMAND_TYPE: {
        ZUC_MBTIO_SEND_COMMAND* sendCmd = (ZUC_MBTIO_SEND_COMMAND*)cmd;
        zucTioMBSendCommand(sendCmd->chnId, sendCmd->command, sendCmd->length);
        break;
    }

    case ZUC_GRAVITY_DIRECTION_TYPE: {
        ZUC_GRAVITY_DIRECTION* task_cmd = (ZUC_GRAVITY_DIRECTION*)cmd;
        // zucSetGravityDirection(task_cmd->robot_id, task_cmd->rpy);
        break;
    }

    case ZUC_TASK_SETUP_TIO_RS485_MODE_TYPE: {
        ZUC_TASK_SETUP_TIO_RS485_MODE* setCmd = (ZUC_TASK_SETUP_TIO_RS485_MODE*)cmd;
        // zucTioSetupRS485ChnMode(setCmd->robot_id, setCmd->id, setCmd->mode);
        break;
    }

    case ZUC_TASK_SETUP_TIO_RS485_COMM_TYPE: {
        ZUC_TASK_SETUP_TIO_RS485_COMM* setCmd = (ZUC_TASK_SETUP_TIO_RS485_COMM*)cmd;
        // zucTioSetupRS485ChnComm(setCmd->robot_id, setCmd->id, setCmd->rtuComm);
        break;
    }

    case ZUC_TASK_SETUP_TIO_PIN_MODE_TYPE: {
        ZUC_TASK_SETUP_TIO_PIN_MODE* setCmd = (ZUC_TASK_SETUP_TIO_PIN_MODE*)cmd;
        // zucTioSetPinMode(setCmd->robot_id, setCmd->type, setCmd->mode);
        break;
    }

    case ZUC_TASK_SETUP_TIO_VOUT_ENA_TYPE: {
        ZUC_TASK_SETUP_TIO_VOUT_ENA* setCmd = (ZUC_TASK_SETUP_TIO_VOUT_ENA*)cmd;
        // zucTioSetVoutEnable(setCmd->robot_id, setCmd->enable);
        break;
    }

    case ZUC_TASK_SETUP_TIO_VOUT_VOL_TYPE: {
        ZUC_TASK_SETUP_TIO_VOUT_VOL* setCmd = (ZUC_TASK_SETUP_TIO_VOUT_VOL*)cmd;
        // zucTioSetVoutVoltage(setCmd->robot_id, setCmd->voltage);
        break;
    }

    // task commands
    case ZUC_TASK_INIT_TYPE: {
        retval = zucTaskInit();
        break;
    }
    case ZUC_MOTION_ABORT_TYPE: {
        auto task_cmd = (ZUC_MOTION_ABORT*)cmd;
        zucMotionAbort(task_cmd->robot_id);
        break;
    }
    case ZUC_TASK_ABORT_TYPE: {
        // abort everything
        if (zucStatus->task.interpState != ZUC_TASK_INTERP_IDLE)
        {
            zucOperatorDisplay(INTERP_EXECUTION_TERMINATED, "Terminate program execution.");
        }
        zucTaskAbort();
        zucStatus->motion.category_one_stop_program_pause = 0;
        usrmotSetEmergencyStopPaused(0);
        mdi_execute_abort();
        zucAbortCleanup(ZUC_ABORT_TASK_ABORT);
        retval = 0;

        // 当脚本程序运行了持续等待IO输入指令，停止程序时，结束等待，避免进入单步调试模式时运行状态异常
        zucStatus->task.delayLeft = 0.0;
        taskExecDelayTimeout = 0.0;
        break;
    }

    // mode and state commands
    case ZUC_TASK_SET_MODE_TYPE: {
        mode_msg = (ZUC_TASK_SET_MODE*)cmd;
        rtapi_print("ZUC_TASK_SET_MODE_TYPE. current mode = %d, target mode = %d\n", zucStatus->task.mode, mode_msg->mode);
        if (zucStatus->task.mode == ZUC_TASK_MODE_AUTO && zucStatus->task.interpState != ZUC_TASK_INTERP_IDLE && mode_msg->mode != ZUC_TASK_MODE_AUTO)
        {
            zucOperatorError(OPERATION_IN_WRONG_MODE,
                             _("Can't switch mode while mode is AUTO and interpreter is not IDLE#{\"fixed_key\":[\"AUTO_NOIDLE\",\"SET_MODE\"]}"));
        }
        else
        {  // we can honour the modeswitch
            if (mode_msg->mode == ZUC_TASK_MODE_MANUAL && zucStatus->task.mode != ZUC_TASK_MODE_MANUAL)
            {
                // leaving auto or mdi mode for manual, abort motion
                zucTaskAbort();
                mdi_execute_abort();
                // without zucTaskPlanClose(), a new run command resumes at
                // aborted line-- feature that may be considered later
                {
                    int was_open = taskplanopen;
                    zucTaskPlanClose();
                    if (zuc_debug & ZUC_DEBUG_INTERP && was_open)
                    {
                        rcs_print("zucTaskPlanClose() called at %s:%d\n", __FILE__, __LINE__);
                    }
                }
                // clear out the pending command
                zucTaskCommand = 0;
                interp_list.clear();
                zucStatus->task.currentLine = 0;
                zucStatus->task.executingLine = 0;
                // clear out the interpreter state
                zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
                zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
                stepping = 0;
                steppingWait = 0;
                // now queue up command to resynch interpreter
                zucTaskQueueCommand(&taskPlanSynchCmd);
            }

            if ((zucStatus->motion.traj.inpos == 0 || zucStatus->motion.on_soft_limit || zucStatus->motion.next_to_limit) &&
                mode_msg->mode == ZUC_TASK_MODE_DRAG)
            {  // target state is drag but without precondition
                if (zucStatus->motion.traj.inpos == 0)
                {
                    zucOperatorError(CANNOT_DRAG_DURING_MOVEMENT, "cannot enter drag mode when robot is in movement");
                }
                else if (zucStatus->motion.on_soft_limit)
                {
                    zucOperatorError(DRAG_CANNOT_ENABLE_ON_LIMIT, "can not enter drag mode while out of range");
                }
                else if (zucStatus->motion.next_to_limit)
                {
                    zucOperatorError(DRAG_CANNOT_ENABLE_ON_LIMIT, "can not enter drag mode while out of range");
                }
                retval = -1;
            }
            else
            {
                retval = zucTaskSetMode(mode_msg->mode);
            }
        }
        break;
    }
    case ZUC_TASK_SET_OPERATION_MODE_TYPE: {
        opmode_msg = (ZUC_TASK_SET_OPERATION_MODE*)cmd;
        if (zucStatus->task.interpState != ZUC_TASK_INTERP_IDLE)
        {
            zucOperatorError(CANNOT_SWITCH_MODE_WHEN_PROGRAM_RUN, ("cannot switch mode when program run"));
            retval = -1;
        }
        else if (zucStatus->motion.traj.inpos == 0)
        {
            zucOperatorError(CANNOT_SWITCH_MODE_DURING_MOVEMENT, ("cannot switch mode when robot is in movement"));
            retval = -1;
        }
        else if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_DRAG)
        {
            zucOperatorError(CANNOT_SWITCH_MODE_IN_DRAG, ("cannot switch mode when robot is in drag"));
            retval = -1;
        }
        else
        {
            retval = zucSetOperationMode((unsigned int)opmode_msg->opmode);
        }
        break;
    }

    case ZUC_TASK_SET_STATE_TYPE:
        state_msg = (ZUC_TASK_SET_STATE*)cmd;
        retval = zucTaskSetState(state_msg->state, -2);
        break;
    case ZUC_AXIS_CMD_POWER_TYPE: {
        auto ucmd = (ZUC_AXIS_POWER*)cmd;
        auto cmd = std::make_shared<axis::AxisPower>(ucmd->axisid, ucmd->enable);
        retval = mot::realtime_cmd_call(cmd);
        break;
    }
    case ZUC_AXIS_CMD_RESET_TYPE: {
        auto ucmd = (ZUC_AXIS_RESET*)cmd;
        auto cmd = std::make_shared<axis::AxisReset>(ucmd->axisid);
        retval = mot::realtime_cmd_call(cmd);
        break;
    }
    case ZUC_TASK_SET_AUTOWORK_TYPE:
        autowork_msg = (ZUC_TASK_SET_AUTOWORK*)cmd;
        // retval = zucTaskSetAutoWork(autowork_msg->robot_id, autowork_msg->enable, autowork_msg->filename);
        break;

    case ZUC_SET_ROBOT_NAME_TYPE:
        // retval = zucSetRobotName(robot_id, ((ZUC_SET_ROBOT_NAME*)cmd)->name);
        break;

    case ZUC_SET_CABINET_ID_TYPE:
        retval = zucSetCabinetId(((ZUC_SET_CABINET_ID*)cmd)->cabinet_id, 1);
        break;

    case ZUC_TASK_SET_SCRIPT_BACKUP_TYPE: {
        // retval = zucSetupScriptBackup(robot_id, ((ZUC_TASK_SET_SCRIPT_BACKUP*)cmd)->enable, ((ZUC_TASK_SET_SCRIPT_BACKUP*)cmd)->interval);
        break;
    }
    case ZUC_SET_CUSTOM_POSE_TYPE: {
        auto task_cmd = (ZUC_SET_CUSTOM_POSE*)cmd;
        int cmdType = task_cmd->type;
        if (cmdType <= 2)
        {
            retval = zucSetCustomPose(task_cmd->robot_id, cmdType, task_cmd->jpos.val);
        }
        else
        {
            retval = zucSetCustomPoseTol(task_cmd->robot_id, task_cmd->poseTol);
        }
        break;
    }

    case ZUC_SET_SERVOJ_PARAMETER_TYPE: {
        ZUC_SET_SERVOJ_PARAMETER* servoCmd = (ZUC_SET_SERVOJ_PARAMETER*)cmd;
        if (zucStatus->motion.traj.mode == 6)
        {
            retval = -1;
            zucOperatorError(OPERATION_IN_WRONG_MODE,
                             _("cannot set servoj filter parameters when it's in servoj mode#{\"fixed_key\":[\"SERVOMODE\",\"SET_SERVOJ_PARAMETER\"]}"));
        }
        else
        {
            zucSetServoMoveFilterType(servoCmd->robot_id, servoCmd->servo_filter_type);
            if (servoCmd->servo_filter_type == 1)
            {
                zucSetServoMoveLPFParam(servoCmd->robot_id, servoCmd->lpf_cutoff_freq);
            }
            else if (servoCmd->servo_filter_type == 2)
            {
                zucSetServoMoveNLFParamJ(servoCmd->robot_id, servoCmd->nlf_vr, servoCmd->nlf_ar, servoCmd->nlf_jr);
            }
            else if (servoCmd->servo_filter_type == 3)
            {
                zucSetServoMoveMMFParam(servoCmd->robot_id, servoCmd->mmf_buf_size, servoCmd->mmf_ka, servoCmd->mmf_kv, servoCmd->mmf_kp);
            }
            else if (servoCmd->servo_filter_type == 4)
            {
                zucSetServoMoveNLFParamC(
                    servoCmd->robot_id, servoCmd->nlf_vp, servoCmd->nlf_ap, servoCmd->nlf_jp, servoCmd->nlf_vr, servoCmd->nlf_ar, servoCmd->nlf_jr);
            }
            else if (servoCmd->servo_filter_type == 5)
            {
                zucSetServoMoveLAVSParamC(servoCmd->robot_id, servoCmd->mmf_buf_size, servoCmd->mmf_kp);
            }
        }
        break;
    }

    case ZUC_SET_NET_EXCEPTION_HANDLE_TYPE: {
        ZUC_SET_NET_EXCEPTION_HANDLE* net = (ZUC_SET_NET_EXCEPTION_HANDLE*)cmd;
        retval = zucSetNetExceptionHandle(net->timeLimit, net->motType);
        zucStatus->netExceptionHandle.timeLimit = net->timeLimit;
        zucStatus->netExceptionHandle.motType = net->motType;
        break;
    }

    case ZUC_SET_SIM_ENABLE_TYPE: {
        auto set_sim_enable = mot::al::MotionProxy::instance().robot(0);
        if (!set_sim_enable.get())
        {
            break;
        }
        ZUC_SET_SIM_ENABLE* setSimEnaCmd = (ZUC_SET_SIM_ENABLE*)cmd;
        if (zucStatus->motion.powered_on)
        {
            zucOperatorError(FAILED_SWITCH_SIM_IF_POWER_ON, "cannot switch sim/real mode with power on");
            retval = -1;
        }
        else
        {
            if (setSimEnaCmd->simEnable && zuc_sim == ZUC_SIM_DISABLED)
            {
                retval = set_sim_enable->zucMotionSetRunMode(RUN_MODE_SIM_ON_REAL);  // simulation on real device
                if (retval == 0)
                    zuc_sim = ZUC_SIM_ENA_ON_DEV;
            }
            else if (!setSimEnaCmd->simEnable && zuc_sim == ZUC_SIM_ENA_ON_DEV)
            {
                retval = set_sim_enable->zucMotionSetRunMode(RUN_MODE_REAL);  // run with real device
                if (retval == 0)
                    zuc_sim = ZUC_SIM_DISABLED;
            }
            else
            {
                zucOperatorError(FAILED_SWITCH_SIM_FAIL, "cannot switch robot model with VM");
                rcs_print("failed to set sim mode: %d\n", setSimEnaCmd->simEnable);
            }
        }
        break;
    }

    case ZUC_SET_SIM_ROBOT_TYPE: {
        ZUC_SET_SIM_ROBOT* setSimRobotCmd = (ZUC_SET_SIM_ROBOT*)cmd;
        if (zucStatus->currSimMode != 1)
        {
            // zucOperatorError(FAILED_SWITCH_SIM_IF_POWER_ON, "cannot switch sim/real mode with power on");
            printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> cannot set robot model unless in sim mode\n");
            retval = -1;
        }
        zucStatus->currSimRobot = setSimRobotCmd->robotModel;
        zucSetSimRobot(setSimRobotCmd->robot_id, setSimRobotCmd->robotModel);
        break;
    }

    case ZUC_SYNC_ZUCSETTINGS_TYPE: {
        rcs_print("ZUC_SYNC_ZUCSETTINGS_TYPE.\n");
        retval = zucSyncZucsettings();
        break;
    }

    case ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_STARTPOS_TYPE: {
        ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_STARTPOS* msg = (ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_STARTPOS*)cmd;
        auto initpayloadidentifytraj = mot::al::MotionProxy::instance().robot(0);
        if (!initpayloadidentifytraj.get())
        {
            break;
        }
        // retval = zucSetPayloadIdentifyTrajStartPos(msg->robot_id, msg->trajId, msg->jointTraj);
        break;
    }

    case ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_ENDPOS_TYPE: {
        auto payload_identify_traj = mot::al::MotionProxy::instance().robot(0);
        if (!payload_identify_traj.get())
        {
            break;
        }
        ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_ENDPOS* msg = (ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_ENDPOS*)cmd;
        // retval = zucSetPayloadIdentifyTrajEndPos(msg->robot_id, msg->trajId, msg->jointTraj);
        break;
    }

    case ZUC_COLLISION_GM_RECORD_ENABLE_TYPE: {
        rcs_print("ZUC_COLLISION_GM_RECORD_ENABLE_TYPE\n");
        ZUC_COLLISION_GM_RECORD_ENABLE* task_cmd = (ZUC_COLLISION_GM_RECORD_ENABLE*)cmd;
        zucSetCollisionGmRecordEnable(task_cmd->gm_record_enable);
        break;
    }

    case ZUC_TASK_START_PAYLOAD_IDENTIFY_TRAJ_TYPE: {
        auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
        if (!robot_motion_task.get())
        {
            break;
        }
        ZUC_TASK_START_PAYLOAD_IDENTIFY_TRAJ* msg = (ZUC_TASK_START_PAYLOAD_IDENTIFY_TRAJ*)cmd;
        if (msg->trajId < 0 || msg->trajId > 3)
        {
            retval = -1;
            zucOperatorError(INVALID_IDENTIFY_TRAJ_INDEX, "Invalid identification trajectory index.");
        }
        else
        {
            zucStatus->motion.identifyWithLoad = msg->withPayload;
            zucStatus->motion.identifyTrajIndex = msg->trajId;
            retval = robot_motion_task->zucStartPayloadIdentifyTraj(msg->trajId, zucStatus->task.payloadIdentifyTraj[msg->trajId], msg->withPayload);
        }
        break;
    }

    case ZUC_TASK_RESET_PAYLOAD_IDENTIFY_STATUS_TYPE: {
        ZUC_TASK_RESET_PAYLOAD_IDENTIFY_STATUS* msg = (ZUC_TASK_RESET_PAYLOAD_IDENTIFY_STATUS*)cmd;
        retval = zucResetPayloadIdentifyStatus(msg->userOpt);
        break;
    }

    case ZUC_GET_PAYLOAD_IDENTIFY_RESULT_TYPE: {
        retval = zucGetPayloadIdentifyResult();
        if (retval == -1)
        {
            zucOperatorError(EEROR_GET_PAYLOAD_RESULT, "Get Identify Result Failed!");
        }
        break;
    }

    case ZUC_STOP_IDENTIFY_FRICTION_TYPE: {
        auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
        if (!robot_motion_task.get())
        {
            break;
        }
        ZUC_STOP_IDENTIFY_FRICTION* task_cmd = (ZUC_STOP_IDENTIFY_FRICTION*)cmd;
        // retval = zucStopIdentifyFriction(task_cmd->robot_id, task_cmd->friction_identify_joint_index);
        break;
    }

    case ZUC_START_IDENTIFY_FRICTION_TYPE: {
        auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
        if (!robot_motion_task.get())
        {
            break;
        }
        ZUC_START_IDENTIFY_FRICTION* task_cmd = (ZUC_START_IDENTIFY_FRICTION*)cmd;
        // retval = zucStartIdentifyFriction(task_cmd->robot_id, task_cmd->friction_identify_joint_index);
        break;
    }

    case ZUC_IDENTIFY_FRICTION_END_POS_TYPE: {
        ZUC_IDENTIFY_FRICTION_END_POS* task_cmd = (ZUC_IDENTIFY_FRICTION_END_POS*)cmd;
        // zucSetIdentifyFrictionEndPos(task_cmd->robot_id, task_cmd->friction_identify_joint_index, task_cmd->friction_identify_end_pos);
        break;
    }

    case ZUC_IDENTIFY_FRICTION_START_POS_TYPE: {
        ZUC_IDENTIFY_FRICTION_START_POS* task_cmd = (ZUC_IDENTIFY_FRICTION_START_POS*)cmd;
        // zucSetIdentifyFrictionStartPos(task_cmd->robot_id, task_cmd->friction_identify_joint_index, task_cmd->friction_identify_start_pos);
        break;
    }

    case ZUC_SERVO_DYNAMICS_PARA_TYPE: {
        ZUC_SERVO_DYNAMICS_PARA* task_cmd = (ZUC_SERVO_DYNAMICS_PARA*)cmd;
        zucSetServoDynamicsPara(task_cmd->robot_id, task_cmd->servo_dynamics_para);
        break;
    }

    case ZUC_SERVO_FRICTION_TYPE: {
        ZUC_SERVO_FRICTION* task_cmd = (ZUC_SERVO_FRICTION*)cmd;
        zucSetServoFriction(task_cmd->robot_id, task_cmd->friction_para, task_cmd->jnt_id);
        break;
    }

    case ZUC_COLLISION_SENSITIVITY_TYPE: {
        ZUC_COLLISION_SENSITIVITY* task_cmd = (ZUC_COLLISION_SENSITIVITY*)cmd;
        zucSetCollisionSensitivity(task_cmd->robot_id, task_cmd->joint_sensitivity);
        break;
    }

    case ZUC_FORCE_LIMIT_VALUE_TYPE: {
        ZUC_FORCE_LIMIT_VALUE* task_cmd = (ZUC_FORCE_LIMIT_VALUE*)cmd;
        zucSetForceLimitValue(task_cmd->robot_id, task_cmd->force_limit_value);

        // 根据连续等级计算离散等级
        ServoParam servoParam;
        servoParam.paramType = ServoParam::CLSN_SENSITIVITY;
        if (task_cmd->force_limit_value < 19)
        {
            servoParam.paramVal.clsn_sensitivity = 1;
        }
        else if (task_cmd->force_limit_value < 38)
        {
            servoParam.paramVal.clsn_sensitivity = 2;
        }
        else if (task_cmd->force_limit_value < 57)
        {
            servoParam.paramVal.clsn_sensitivity = 3;
        }
        else if (task_cmd->force_limit_value < 76)
        {
            servoParam.paramVal.clsn_sensitivity = 4;
        }
        else if (task_cmd->force_limit_value < 95)
        {
            servoParam.paramVal.clsn_sensitivity = 5;
        }
        else
        {
            servoParam.paramVal.clsn_sensitivity = 7;
        }

        // zucSetServoParam(robot_id, servoParam);
        break;
    }

    case ZUC_COLLISION_DETECTION_ENABLE_TYPE: {
        ZUC_COLLISION_DETECTION_ENABLE* task_cmd = (ZUC_COLLISION_DETECTION_ENABLE*)cmd;
        zucSetCollisionDetectionEnable(task_cmd->robot_id, task_cmd->collision_detection_enable);
        break;
    }

    case ZUC_RS485_MESSAGE_TYPE: {
        ZUC_RS485_MESSAGE* task_cmd = (ZUC_RS485_MESSAGE*)cmd;
        // zucGetRs485Message(task_cmd->robot_id, task_cmd->chn, task_cmd->len);
        break;
    }

    case ZUC_TIO_RS485_SIG_PROG_STAT_TYPE: {
        ZUC_TIO_RS485_SIG_PROG_STAT* task_cmd = (ZUC_TIO_RS485_SIG_PROG_STAT*)cmd;
        // zucSetTioRs485SigProgStat(task_cmd->robot_id, task_cmd->sig_name, task_cmd->prog_stat);
        break;
    }

    case ZUC_DRAG_CONTROL_ENABLE_TYPE: {
        ZUC_DRAG_CONTROL_ENABLE* task_cmd = (ZUC_DRAG_CONTROL_ENABLE*)cmd;
        // zucSetDragControlEnable(task_cmd->robot_id, task_cmd->drag_control_enable);
        break;
    }

    case ZUC_DRAG_CONTROL_FC_COMPENSATE_TYPE: {
        ZUC_DRAG_CONTROL_FC_COMPENSATE* task_cmd = (ZUC_DRAG_CONTROL_FC_COMPENSATE*)cmd;
        // zucSetDragControlFcCompensate(task_cmd->robot_id, task_cmd->drag_control_fc_compensate);
        break;
    }

    case ZUC_DRAG_CONTROL_FV_COMPENSATE_TYPE: {
        ZUC_DRAG_CONTROL_FV_COMPENSATE* task_cmd = (ZUC_DRAG_CONTROL_FV_COMPENSATE*)cmd;
        // zucSetDragControlFvCompensate(task_cmd->robot_id, task_cmd->drag_control_fv_compensate);
        break;
    }

    case ZUC_DRAG_CONTROL_FILTER_FACTOR_TYPE: {
        ZUC_DRAG_CONTROL_FILTER_FACTOR* task_cmd = (ZUC_DRAG_CONTROL_FILTER_FACTOR*)cmd;
        // zucSetDragControlFilterFactor(task_cmd->robot_id, task_cmd->drag_control_filter_factor);
        break;
    }

    case ZUC_DRAG_CONTROL_DITHER_FACTOR_TYPE: {
        ZUC_DRAG_CONTROL_DITHER_FACTOR* task_cmd = (ZUC_DRAG_CONTROL_DITHER_FACTOR*)cmd;
        // zucSetDragControlDitherFactor(task_cmd->robot_id, task_cmd->drag_control_dither_factor);
        break;
    }

    case ZUC_TORQUE_CONTROL_ENABLE_TYPE: {
        ZUC_TORQUE_CONTROL_ENABLE* task_cmd = (ZUC_TORQUE_CONTROL_ENABLE*)cmd;
        zucSetTorqueControlEnable(task_cmd->torque_control_enable, task_cmd->torque_last_period);
        break;
    }

    case ZUC_IDENTIFY_CYCLE_TIME_TYPE: {
        ZUC_IDENTIFY_CYCLE_TIME* task_cmd = (ZUC_IDENTIFY_CYCLE_TIME*)cmd;
        // zucSetIdentifyCycleTime(task_cmd->robot_id, task_cmd->identify_cycle_time);
        break;
    }

    case ZUC_TASK_START_DYNAMICS_IDENTIFY_TYPE: {
        retval = zucStartDynamicsIdentify();
        break;
    }

    case ZUC_TASK_STOP_DYNAMICS_IDENTIFY_TYPE: {
        retval = zucStopDynamicsIdentify();
        break;
    }

    // interpreter commands
    case ZUC_TASK_PLAN_CLOSE_TYPE: {
        retval = zucTaskPlanClose();
        if (retval > INTERP_MIN_ERROR)
        {
            zucOperatorError(PROGRAM_FILE_FAILED_CLOSE, _("failed to close file"));
            retval = -1;
        }
        else
        {
            retval = 0;
        }
        break;
    }
    case ZUC_TASK_PLAN_OPEN_TYPE: {
        open_msg = (ZUC_TASK_PLAN_OPEN*)cmd;
        retval = zucTaskPlanOpen(open_msg->file);
        if (retval > INTERP_MIN_ERROR)
        {
            retval = -1;
        }
        if (-1 != retval)
        // {
        //     std::string filePath = std::string(open_msg->file);
        //     std::string progDir = std::string("/usr/etc/jkzuc/scripts");
        //     if (filePath.length() > progDir.length())
        //     {
        //         filePath = std::string(open_msg->file).substr(progDir.length(), -1);
        //     }
        //     zucOperatorError(PROGRAM_FILE_FAILED_OPEN, _("can't open %s #{\"fixed_key\":[\"%s\"]}"), open_msg->file, filePath.c_str());
        // }
        // else
        {
            if (zucStatus->operation_mode == 1)  //auto-mode
            {
                strcpy(zucStatus->task.file_auto_mode, open_msg->file);
            }

            strcpy(zucStatus->task.file_manu_mode, open_msg->file);
            printf(
                ">>operationMode_option: %d,  operationMode: %d,  openFile: %s\n", zucStatus->operation_mode_option, zucStatus->operation_mode, open_msg->file);

            retval = 0;
        }
        break;
    }
    case ZUC_TASK_PLAN_EXECUTE_TYPE: {
        stepping = 0;
        steppingWait = 0;
        execute_msg = (ZUC_TASK_PLAN_EXECUTE*)cmd;
        // if (!all_homed() && !no_force_homing)
        // {  //!no_force_homing = force homing before MDI
        //     retval = -1;
        //     break;
        // }
        if (zucStatus->task.mode != ZUC_TASK_MODE_MDI)
        {
            zucOperatorError(OPERATION_IN_WRONG_MODE, _("Must be in MDI mode to issue MDI command#{\"fixed_key\":[\"NOMDI\",\"TASK_PLAN_EXECUTE\"]}"));
            retval = -1;
            break;
        }
        // track interpState also during MDI - it might be an oword sub call
        zucStatus->task.interpState = ZUC_TASK_INTERP_READING;

        if (execute_msg->command[0] != 0)
        {
            char* command = execute_msg->command;
            rcs_print("execute_msg>>>, %s\n", command);
            if (command[0] == (char)0xff)
            {
                // Empty command recieved. Consider it is NULL
                command = NULL;
            }
            else
            {
                // record initial MDI command
                strcpy(zucStatus->task.command, execute_msg->command);
            }

            int level = zucTaskPlanLevel();
            if (zucStatus->task.mode == ZUC_TASK_MODE_MDI)
            {
                if (mdi_execute_level < 0)
                    mdi_execute_level = level;
            }

            execRetval = zucTaskPlanExecute(command, 0);

            level = zucTaskPlanLevel();

            if (zucStatus->task.mode == ZUC_TASK_MODE_MDI)
            {
                if (mdi_execute_level == level)
                {
                    mdi_execute_level = -1;
                }
                else if (level > 0)
                {
                    // Still insude call. Need another execute(0) call
                    // but only if we didnt encounter an error
                    if (execRetval == INTERP_ERROR)
                    {
                        mdi_execute_next = 0;
                    }
                    else
                    {
                        mdi_execute_next = 1;
                    }
                }
            }
            switch (execRetval)
            {
            case INTERP_EXECUTE_FINISH:
                // Flag MDI wait
                mdi_execute_wait = 1;
                // need to flush execution, so signify no more readinguntil all is done
                zucTaskPlanSetWait();
                // and resynch the interpreter WM
                zucTaskQueueCommand(&taskPlanSynchCmd);
                // it's success, so retval really is 0
                retval = 0;
                break;

            case INTERP_ERROR:
                interp_list.clear();
                zucTaskAbort();
                mdi_execute_abort();  // sets zucStatus->task.interpState to  ZUC_TASK_INTERP_IDLE
                zucAbortCleanup(ZUC_ABORT_INTERPRETER_ERROR_MDI, "interpreter error during MDI");
                retval = -1;
                break;

            case INTERP_EXIT:
            case INTERP_ENDFILE:
            case INTERP_FILE_NOT_OPEN:
                // this caused the error msg on M2 in MDI mode - execRetval == INTERP_EXIT which is would be ok (I think). mah
                retval = -1;
                break;

            default:
                // other codes are OK
                retval = 0;
            }
        }
        break;
    }
    case ZUC_TASK_PLAN_RUN_TYPE: {
        // if (!all_homed() && !no_force_homing)
        // {  //!no_force_homing = force homing before Auto
        //     retval = -1;
        //     break;
        // }
        if (!zucStatus->motion.is_enabled)  // 如果没有使能
        {
            zucOperatorError(ROBOT_NOT_ENABLED, "[start_program]  robot is not enabled");
            retval = -1;
            break;
        }

        if (true == zucStatus->io.func_io_req.program_stop_req || true == zucStatus->io.func_io_req.program_pause_req)  // funcio  program_stop_req is true
        {
            zucOperatorError(ERR_FDI_PROGRUN_LOCKED, "start-program function is locked by sprogram_stop_req of funcIO.");
            retval = -1;
            break;
        }
        if (zucStatus->motion.three_position_enable_limit == 1)
        {
            zucOperatorError(ERROR_THREE_POSITION_ENABLING_LIMIT, _("Can't debug when the three positions enable limits"));
            retval = -1;
            break;
        }

        int statServo = ServoStatistic::get()->check(true);
        if (statServo)
        {
            ServoStatistic::get()->printServoStatistic();
            zucOperatorWarning(
                SERVO_ENCODE_ERROR, "joint encode statistic warning#{\"fixed_key\":[\"%s\"]}", ServoStatistic::get()->formatDaliyWarnStr().c_str());
            // 此处不用返回错误，让用户程序启动
        }
        stepping = 0;
        steppingWait = 0;
        if (!taskplanopen)
        {
            // zucTaskAbort  --->  zucTaskPlanClose  会将 taskplanopen =0，   所以ZUC_TASK_PLAN_OPEN 指令后，ZUC_TASK_PLAN_RUN时 大部分情况都还需要再 open 一次。
            if (zucStatus->task.file[0] != 0)  // jks 文件成功打开
            {
                printf("ZUC_TASK_PLAN_RUN: open %s\n", zucStatus->task.file);
                if (zucTaskPlanOpen(zucStatus->task.file) > INTERP_MIN_ERROR)
                {
                    retval = -1;
                    break;
                }
            }
            else
            {
                retval = -1;
                break;
            }
        }
        run_msg = (ZUC_TASK_PLAN_RUN*)cmd;
        programStartLine = run_msg->line;
        zucTaskSetStartLine(programStartLine);
        zucStatus->task.interpreter_errcode = 0;
        zucStatus->task.interpState = ZUC_TASK_INTERP_READING;
        zucStatus->task.task_paused = 0;
        retval = 0;

        zucStatus->task.progInExec = 1;
        if (zucStatus->stepStatus.isSteping && zucStatus->stepStatus.programRunTrigger)
            zucStatus->stepStatus.stepEnd = 1;
        else
            zucStatus->stepStatus.programRunTrigger = 1;
        break;
    }

    case ZUC_TASK_PLAN_PAUSE_TYPE: {
        zucTrajPause(-1);
        if (zucStatus->task.interpState != ZUC_TASK_INTERP_PAUSED)
        {
            interpResumeState = zucStatus->task.interpState;
        }
        if (zucStatus->isSteppingMode && zucStatus->steppingState == ZUC_TASK_INTERP_READING)
        {
            zucStatus->steppingState = ZUC_TASK_INTERP_PAUSED;
        }
        zucStatus->task.interpState = ZUC_TASK_INTERP_PAUSED;
        zucStatus->task.task_paused = 1;
        retval = 0;
        thread_pause_flag = true;
        break;
    }
    case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE: {
        auto plan_optional_stop_type = mot::al::MotionProxy::instance().robot(0);
        if (!plan_optional_stop_type.get())
        {
            break;
        }
        if (GET_OPTIONAL_PROGRAM_STOP() == ON)
        {
            plan_optional_stop_type->do_pause();
            if (zucStatus->task.interpState != ZUC_TASK_INTERP_PAUSED)
            {
                interpResumeState = zucStatus->task.interpState;
            }
            zucStatus->task.interpState = ZUC_TASK_INTERP_PAUSED;
            zucStatus->task.task_paused = 1;
        }
        retval = 0;
        break;
    }
    case ZUC_TASK_CALC_TCP_TYPE: {
        calc_tcp_msg = (ZUC_TASK_CALC_TCP*)cmd;
        auto calc_tcp = mot::al::MotionProxy::instance().robot(0);
        if (!calc_tcp.get())
        {
            break;
        }

        if (calc_tcp_msg->methodtype == 0)
        {
            retval = calc_tcp->zucTrajCalcTcp(calc_tcp_msg->id,
                                              calc_tcp_msg->saveType,
                                              calc_tcp_msg->statusFlag,
                                              calc_tcp_msg->jntpoint1,
                                              calc_tcp_msg->jntpoint2,
                                              calc_tcp_msg->jntpoint3,
                                              calc_tcp_msg->jntpoint4);
        }
        else if (calc_tcp_msg->methodtype == 1)
        {
            retval = calc_tcp->zucTrajCalcTcp_six(calc_tcp_msg->id,
                                                  calc_tcp_msg->saveType,
                                                  calc_tcp_msg->statusFlag,
                                                  calc_tcp_msg->jntpoint1,
                                                  calc_tcp_msg->jntpoint2,
                                                  calc_tcp_msg->jntpoint3,
                                                  calc_tcp_msg->jntpoint4,
                                                  calc_tcp_msg->jntpoint5,
                                                  calc_tcp_msg->jntpoint6);
        }
        break;
    }
    case ZUC_TASK_CALC_SAFEATT_TYPE:
        calc_safeatt_msg = (ZUC_TASK_CALC_SAFEATT*)cmd;
        retval = zucTrajCalcSafeAtt(&calc_safeatt_msg->cartpos1,
                                    &calc_safeatt_msg->cartpos2,
                                    &calc_safeatt_msg->cartpos3,
                                    &calc_safeatt_msg->tilt,
                                    &calc_safeatt_msg->pan,
                                    &calc_safeatt_msg->tipChoice);
        break;

    case ZUC_TASK_CALC_USER_TYPE: {
        calc_user_msg = (ZUC_TASK_CALC_USER*)cmd;
        auto calc_user = mot::al::MotionProxy::instance().robot(0);
        if (!calc_user.get())
        {
            break;
        }
        retval = calc_user->zucTrajCalcUser(calc_user_msg->id,
                                            calc_user_msg->saveType,
                                            calc_user_msg->statusFlag,
                                            calc_user_msg->jntpoint1,
                                            calc_user_msg->jntpoint2,
                                            calc_user_msg->jntpoint3);
        break;
    }
    case ZUC_TASK_USER_DEFINED_VARIABLE_TYPE:
        user_defined_var = (ZUC_TASK_SET_USER_DEFINED_VARIABLE*)cmd;
        retval = zucTaskSetUserDefinedVar(user_defined_var->id, user_defined_var->value);
        break;

    case ZUC_TASK_PLAN_RESUME_TYPE: {
        auto plan_resume_type = mot::al::MotionProxy::instance().robot(0);
        if (!plan_resume_type.get())
        {
            break;
        }
        retval = 0;
        if (true == zucStatus->io.func_io_req.program_pause_req)  // funcio  program_spause_req is true
        {
            zucOperatorError(ERR_FDI_RESUME_LOCKED, "resume-program function is locked by program_pause_req of funcIO.");
            retval = -1;
            break;
        }

        if (zucStatus->task.task_paused && !zucStatus->motion.protective_stop)
        {
            if (zucStatus->stepStatus.isSteping)  // program step mode
            {
                if (steppingLine == 0 && zucStatus->motion.traj.id != 0)
                    steppingLine = zucStatus->motion.traj.id;
                else if ((steppingLine == 0 && zucStatus->motion.traj.id == 0) || steppingLine != zucStatus->motion.traj.id)
                    break;
                plan_resume_type->zucTrajStep();
                zucStatus->steppingState = ZUC_TASK_INTERP_READING;
                zucStatus->task.interpState = (enum ZUC_TASK_INTERP_ENUM)interpResumeState;
            }
            else
            {
                zucTrajResume(-1);  // 默认恢复所有机器人运动
                zucStatus->task.interpState = (enum ZUC_TASK_INTERP_ENUM)interpResumeState;
                zucStatus->task.task_paused = 0;
                steppingWait = 0;
                stepping = 0;
            }
        }
        thread_pause_flag = false;
        pthread_cond_signal(&thread_cond);
        break;
    }
    case ZUC_TASK_PLAN_END_TYPE:
        zucTaskAbort();
        zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
        retval = 0;
        break;

    case ZUC_TASK_PLAN_INIT_TYPE:
        retval = zucTaskPlanInit();
        if (retval > INTERP_MIN_ERROR)
        {
            retval = -1;
        }
        break;

    case ZUC_TASK_PLAN_SYNCH_TYPE:
        retval = zucTaskPlanSynch();
        if (retval > INTERP_MIN_ERROR)
        {
            retval = -1;
        }
        break;

    case ZUC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE:
        os_msg = (ZUC_TASK_PLAN_SET_OPTIONAL_STOP*)cmd;
        zucTaskPlanSetOptionalStop(os_msg->state);
        retval = 0;
        break;

    case ZUC_TASK_PLAN_SET_BLOCK_DELETE_TYPE:
        bd_msg = (ZUC_TASK_PLAN_SET_BLOCK_DELETE*)cmd;
        zucTaskPlanSetBlockDelete(bd_msg->state);
        retval = 0;
        break;

    case ZUC_SET_MUTI_TOOL_OFFSET_TYPE: {
        set_muti_tool_offset_msg = (ZUC_SET_MUTI_TOOL_OFFSET*)cmd;
        retval = zucTrajSetRobotToolOffset(
            set_muti_tool_offset_msg->robotToolOffset, set_muti_tool_offset_msg->method, set_muti_tool_offset_msg->id, set_muti_tool_offset_msg->name);
        break;
    }
    case ZUC_SET_MUTI_USER_OFFSET_TYPE: {
        set_muti_user_offset_msg = (ZUC_SET_MUTI_USER_OFFSET*)cmd;
        retval = zucTrajSetRobotUserOffset(
            set_muti_user_offset_msg->robotUserOffset, set_muti_user_offset_msg->method, set_muti_user_offset_msg->id, set_muti_user_offset_msg->name);
        break;
    }
    case ZUC_SET_TOOL_ID_TYPE:
        set_tool_id_msg = (ZUC_SET_TOOL_ID*)cmd;
        retval = zucTrajSetRobotToolID(set_tool_id_msg->robot_id, set_tool_id_msg->id);
        break;

    case ZUC_SET_USER_ID_TYPE:
        set_user_id_msg = (ZUC_SET_USER_ID*)cmd;
        retval = zucTrajSetRobotUserID(set_user_id_msg->robot_id, set_user_id_msg->id);
        break;

    case ZUC_CONVEYOR_ENABLE_TYPE: {
        auto conveyor_enable = mot::al::MotionProxy::instance().robot(0);
        if (!conveyor_enable.get())
        {
            break;
        }
        conveyor_enable_msg = (ZUC_CONVEYOR_ENABLE*)cmd;
        if (conveyor_enable_msg->convyr_type == 1)
        {
            int ret = conveyor_enable->zucTrajConveyorLinearEnable(conveyor_enable_msg->x,
                                                                   conveyor_enable_msg->y,
                                                                   conveyor_enable_msg->z,
                                                                   conveyor_enable_msg->pulseEquivalent,
                                                                   conveyor_enable_msg->convyr_type,
                                                                   conveyor_enable_msg->maxDistance);
            if (ret == -1)
            {
                zucTaskAbort();
            }
        }
        else if (conveyor_enable_msg->convyr_type == 2)
        {
            int ret = conveyor_enable->zucTrajConveyorCircularEnable(conveyor_enable_msg->p1x,
                                                                     conveyor_enable_msg->p1y,
                                                                     conveyor_enable_msg->p1z,
                                                                     conveyor_enable_msg->p2x,
                                                                     conveyor_enable_msg->p2y,
                                                                     conveyor_enable_msg->p2z,
                                                                     conveyor_enable_msg->p3x,
                                                                     conveyor_enable_msg->p3y,
                                                                     conveyor_enable_msg->p3z,
                                                                     conveyor_enable_msg->pulseEquivalent,
                                                                     conveyor_enable_msg->convyr_type,
                                                                     conveyor_enable_msg->rotate_tool,
                                                                     conveyor_enable_msg->maxDistance);
            if (ret == -1)
            {
                zucTaskAbort();
            }
        }

        break;
    }
    case ZUC_CONVEYOR_DISABLE_TYPE: {
        auto conveyor_disable = mot::al::MotionProxy::instance().robot(0);
        if (!conveyor_disable.get())
        {
            break;
        }
        conveyor_disable_msg = (ZUC_CONVEYOR_DISABLE*)cmd;
        conveyor_disable->zucTrajConveyorDisable();
        zucTaskQueueCommand(&taskPlanSynchCmd);
        break;
    }
    case ZUC_SET_COLLISION_OPTION_TYPE: {
        ZUC_SET_COLLISION_OPTION* clsnOptCmd = (ZUC_SET_COLLISION_OPTION*)cmd;
        zucSetCollisionOption(clsnOptCmd->robot_id,
                              clsnOptCmd->clsnReactTypeWhenProgRun,
                              clsnOptCmd->clsnReactionReboudnAng,
                              clsnOptCmd->clsnReactionReboudnLen,
                              clsnOptCmd->clsnReactionReboudnJAcc,
                              clsnOptCmd->clsnReactionReboudnJVel,
                              clsnOptCmd->clsnReactionReboudnLAcc,
                              clsnOptCmd->clsnReactionReboudnLVel,
                              clsnOptCmd->clsnDetectMethod);

        break;
    }

    case ZUC_SET_DRAG_ENABLE_TYPE: {
        ZUC_SET_DRAG_ENABLE* task_cmd = (ZUC_SET_DRAG_ENABLE*)cmd;
        zucSetDragEnable(task_cmd->robot_id, task_cmd->enable);
        break;
    }

    case ZUC_SET_ROBOT_PAYLOAD_TYPE: {
        ZUC_SET_ROBOT_PAYLOAD* task_cmd = (ZUC_SET_ROBOT_PAYLOAD*)cmd;
        zucSetRobotPayload(task_cmd->robot_id, task_cmd->id);
        break;
    }

    case ZUC_SERVO_HOME_TYPE: {
        ZUC_SERVO_HOME* task_cmd = (ZUC_SERVO_HOME*)cmd;
        zucServoHome(task_cmd->robot_id, task_cmd->joint);
        break;
    }

    case ZUC_CLEAR_ERROR_TYPE: {
        ZUC_CLEAR_ERROR* task_cmd = (ZUC_CLEAR_ERROR*)cmd;
        zucClearError(task_cmd->robot_id);
        break;
    }

    case ZUC_SET_BASE_OFFSET_TYPE: {
        ZUC_SET_BASE_OFFSET* task_cmd = (ZUC_SET_BASE_OFFSET*)cmd;
        zucSetBaseOffset(task_cmd->robot_id, task_cmd->base_offset);
        break;
    }

    default:  // unrecognized command
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print_error("ignoring issue of unknown command %d:%s\n", (int)cmd->type, zucCommand->msg_name());
        }
        retval = 0;  // don't consider this an error
        break;
    }

    if (retval == -1)
    {
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print_error("error executing command %d:%s\n", (int)cmd->type, zucCommand->msg_name());
        }
    }
    /* debug */
    if ((zuc_debug & ZUC_DEBUG_TASK_ISSUE) && retval)
    {
        rcs_print("zucTaskIssueCommand() returning: %d\n", retval);
    }
    return retval;
}

static int zucTaskSetGrpcErrCode(NMLmsg* cmd)
{
    if (0 == cmd)
    {
        return 0;
    }
    zucStatus->grpc_errtype = ((ZUC_SET_GRPC_ERRCODE*)cmd)->grpc_errtype;
    zucStatus->grpc_errcode = ((ZUC_SET_GRPC_ERRCODE*)cmd)->grpc_errcode;
    strncpy(zucStatus->grpc_errmsg, ((ZUC_SET_GRPC_ERRCODE*)cmd)->grpc_errmsg, LINELEN);
    zucStatus->repeatflag = ((ZUC_SET_GRPC_ERRCODE*)cmd)->repeatflag;
    if (((ZUC_SET_GRPC_ERRCODE*)cmd)->saveFlag)  // 记录到errorlog文件
    {
        switch (zucStatus->grpc_errtype)
        {
        case 11:  // ZUC_OPERATOR_ERROR_TYPE
        case 14:  //GRPC_RESPONSE_ERROR_TYPE
            zucLogError(zucStatus->grpc_errcode, zucStatus->grpc_errmsg);
            break;
        case 12:  // ZUC_OPERATOR_TEXT_TYPE
        case 15:  //GRPC_RESPONSE_WARNING_TYPE
            zucLogWarning(zucStatus->grpc_errcode, zucStatus->grpc_errmsg);
            break;
        case 13:  // ZUC_OPERATOR_DISPLAY_TYPE
        case 16:  //GRPC_RESPONSE_INFO_TYPE
            zucLogInfo(zucStatus->grpc_errcode, zucStatus->grpc_errmsg);
            break;
        default:
            break;
        }
    }
    return 0;
}

// static int updateLicense()
// {
// 	static int last_hasp_status = 0;
//     hasp_handle_t   handle;
//     int hasp_status = (hasp_login(HASP_DEFAULT_FID, (hasp_vendor_code_t)vendor_code, &handle) == HASP_STATUS_OK) ? 0 : -1;
// 	if(hasp_status != last_hasp_status) {
// 		if(hasp_status == HASP_STATUS_OK) {
// 			// set function license
// 			rcs_print("licence ok\n");
// 			zucStatus->func_licence = 0x01;
// 		} else {
// 			// reset function license
// 			rcs_print("licence failed, %d\n", hasp_status);
// 			zucStatus->func_licence = 0x00;
// 		}
// 	}
// 	last_hasp_status = hasp_status;
// }

/*
 zucTaskCheckPostconditions() is called for commands on the interp_list.
 Immediate commands, i.e., commands sent from calls to zucTaskIssueCommand()
 in zucTaskPlan() directly, are not handled here.

 The return value is a state for zucTaskExecute() to wait on, e.g.,
 ZUC_TASK_EXEC_WAITING_FOR_MOTION, after the command has finished and
 before any other commands can be sent out.
 */
static int zucTaskCheckPostconditions(NMLmsg* cmd)
{
    if (0 == cmd)
    {
        return ZUC_TASK_EXEC_DONE;
    }

    switch (cmd->type)
    {
    case ZUC_OPERATOR_ERROR_TYPE:
    case ZUC_OPERATOR_TEXT_TYPE:
    case ZUC_OPERATOR_DISPLAY_TYPE:
        return ZUC_TASK_EXEC_DONE;
        break;

    case ZUC_SYSTEM_CMD_TYPE:
        return ZUC_TASK_EXEC_WAITING_FOR_SYSTEM_CMD;
        break;

    case ZUC_TRAJ_LINEAR_MOVE_TYPE:
    case ZUC_TRAJ_CIRCULAR_MOVE_TYPE:
    case ZUC_TRAJ_JOINT_MOVE_TYPE:
    case ZUC_TRAJ_MOVC_TYPE:
    case ZUC_TRAJ_MOVS_TYPE:
    case ZUC_TOPPRA_FINISH_LEFT_TC_TYPE:
    case ZUC_TOPPRA_FIRST_COMMAND_TYPE:
    case ZUC_TRAJ_SET_ACCELERATION_TYPE:
    case ZUC_TRAJ_RIGID_TAP_TYPE:
    case ZUC_TRAJ_SET_TELEOP_ENABLE_TYPE:
    case ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE_TYPE:
    case ZUC_TRAJ_SET_SERVOJOP_ENABLE_TYPE:
    case ZUC_TRAJ_SET_ADMITOP_ENABLE_TYPE:
    case ZUC_TRAJ_SET_COMPLIANCE_ENABLE_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_FRAME_TYPE:
    case ZUC_TRAJ_DISABLE_FORCE_CONTROL_TYPE:
    case ZUC_TRAJ_SET_FO_ENABLE_TYPE:
    case ZUC_TRAJ_SET_FH_ENABLE_TYPE:
    case ZUC_TRAJ_SET_ADMITTANCE_CONFIG_TYPE:
    case ZUC_TRAJ_SET_END_FORCE_CONDITION_TYPE:
        return ZUC_TASK_EXEC_DONE;
        break;

    case ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET_TYPE:
    case ZUC_TRAJ_SET_ROBOT_USER_FRAME_TYPE:
    case ZUC_SET_MUTI_TOOL_OFFSET_TYPE:
    case ZUC_SET_TOOL_ID_TYPE:
    case ZUC_TORQUE_CONTROL_ENABLE_TYPE:
    case ZUC_SET_MUTI_USER_OFFSET_TYPE:
    case ZUC_SET_USER_ID_TYPE:
    case ZUC_TRAJ_SET_PAYLOAD_TYPE:
    case ZUC_TRAJ_CALIB_DHPARAM_TYPE:
        return ZUC_TASK_EXEC_DONE;
        break;

    case ZUC_TASK_PLAN_RUN_TYPE:
    case ZUC_TASK_PLAN_PAUSE_TYPE:
    case ZUC_TASK_PLAN_END_TYPE:
    case ZUC_TASK_PLAN_INIT_TYPE:
    case ZUC_TASK_PLAN_SYNCH_TYPE:
    case ZUC_TASK_PLAN_EXECUTE_TYPE:
    case ZUC_TASK_PLAN_OPTIONAL_STOP_TYPE:
        return ZUC_TASK_EXEC_DONE;
        break;

    case ZUC_TRAJ_DELAY_TYPE:
    case ZUC_AUX_INPUT_WAIT_TYPE:
        return ZUC_TASK_EXEC_WAITING_FOR_DELAY;
        break;

    case ZUC_MOTION_SET_AOUT_TYPE:
    case ZUC_MOTION_SET_DOUT_TYPE:
    case ZUC_MOTION_ADAPTIVE_TYPE:
    case ZUC_MOTION_EXIT_PROTECT_TYPE:
    case ZUC_MOTION_PERCENTAGE_MODE_TYPE:
    case ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE_TYPE:
    case ZUC_MOTION_SET_FREE_BUTTON_OPT_TYPE:
    case ZUC_MOTION_SET_POINT_BUTTON_OPT_TYPE:
    case ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT_TYPE:
    case ZUC_MOTION_KINE_INVERSE_TYPE:
    case ZUC_MOTION_KINE_FORWARD_TYPE:
    case ZUC_MOTION_ONE_KEY_OP_TYPE:
    case ZUC_SET_SERVO_PARAM_TYPE:
    case ZUC_ROBOT_SAFETY_TYPE:
    case ZUC_SET_ROBOT_SAFETY_CUSTOMIZE_TYPE:
    case Robot_7_DOF_INV_INFORM_TYPE:
    case Robot_SET_FULL_DH_FLAG_TYPE:
    case Robot_SET_7_DOF_KINE_PARAM_ALPHA_K_TYPE:
    case ZUC_REDUCE_SAFETY_TYPE:
    case ZUC_DRAG_SPEED_LIMIT_TYPE:
    case ZUC_MOTION_SET_CAPTURE_TYPE:
    case ZUC_MOTION_SET_HOME_VEL_TYPE:
        return ZUC_TASK_EXEC_DONE;
        break;
    default:
        // unrecognized command
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print_error("postconditions: unrecognized command %d:%s\n", (int)cmd->type, zucCommand->msg_name());
        }
        return ZUC_TASK_EXEC_DONE;
        break;
    }
    return ZUC_TASK_EXEC_DONE;  // unreached
}

/*
 STEPPING_CHECK() is a macro that prefaces a switch-case with a check
 for stepping. If stepping is active, it waits until the step has been
 given, then falls through to the rest of the case statement.
 */

#define STEPPING_CHECK()                                      \
    if (stepping)                                             \
    {                                                         \
        if (!steppingWait)                                    \
        {                                                     \
            steppingWait = 1;                                 \
            steppedLine = zucStatus->task.executingLine;      \
        }                                                     \
        else                                                  \
        {                                                     \
            if (zucStatus->task.executingLine != steppedLine) \
            {                                                 \
                break;                                        \
            }                                                 \
        }                                                     \
    }

static int subThreadWaitInputExecute(ThreadInterpInfo* pThreadInterpInfo)
{
    double taskExecDelayTimeout = pThreadInterpInfo->auxInputWaitMsg.taskExecDelayTimeout;
    int zucAuxInputWaitIndex = pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitIndex;
    int zucAuxInputWaitType = pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitType;
    int zucAuxInputWaitDIType = pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitDIType;
    // check if delay has passed
    if (etime() >= taskExecDelayTimeout)
    {
        pThreadInterpInfo->auxInputWaitMsg.waiting = false;
        return 0;
    }
    // delay can be also be because we wait for an input
    // if the index is set (not -1)
    if (zucAuxInputWaitIndex >= 0)
    {
        switch (zucAuxInputWaitType)
        {
        case WAIT_MODE_HIGH: {
            unsigned char eip_di_hi = 1;
            if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 1)
            {
                int byteNum = zucAuxInputWaitIndex / 8;
                zucAuxInputWaitIndex -= byteNum * 8;
                eip_di_hi = 0x01 & (0xff & (zucStatus->io.synch_eip_in[byteNum] >> zucAuxInputWaitIndex));
            }
            else if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 0)
            {
                eip_di_hi = zucStatus->io.synch_eip_adpt_di[zucAuxInputWaitIndex];
            }
            if ((zucAuxInputWaitDIType == DIGITAL_INPUT && zucStatus->io.synch_di[zucAuxInputWaitIndex] != 0) ||
                (zucAuxInputWaitDIType == TIO_DIGITAL_INPUT && (zucStatus->tio[0].tioStatus.DI_buffer & (1 << zucAuxInputWaitIndex))) ||
                (zucAuxInputWaitDIType == EXTIO_DIGITAL_INPUT && zucStatus->io.extio.extIOStat.extDI[zucAuxInputWaitIndex] != 0) ||
                (zucAuxInputWaitDIType == MB_SLAVE_DIGITAL_INPUT && zucStatus->io.synch_mb_slave_di[zucAuxInputWaitIndex] != 0) ||
                (zucAuxInputWaitDIType == PN_DIGITAL_INPUT && zucStatus->io.synch_pn_dev_di[zucAuxInputWaitIndex] != 0) ||
                (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && eip_di_hi != 0))
            {
                pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitIndex = -1;
                pThreadInterpInfo->auxInputWaitMsg.waiting = false;
            }
            break;
        }
        case WAIT_MODE_RISE:
            if (zucStatus->io.synch_di[zucAuxInputWaitIndex] == 0)
            {
                pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitType = WAIT_MODE_HIGH;
            }
            break;

        case WAIT_MODE_LOW: {
            unsigned char eip_di_low = 0;
            if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 1)
            {
                int byteNum = zucAuxInputWaitIndex / 8;
                zucAuxInputWaitIndex -= byteNum * 8;
                eip_di_low = 0x01 & (0xff & (zucStatus->io.synch_eip_in[byteNum] >> zucAuxInputWaitIndex));
            }
            else if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 0)
            {
                eip_di_low = zucStatus->io.synch_eip_adpt_di[zucAuxInputWaitIndex];
            }
            if ((zucAuxInputWaitDIType == DIGITAL_INPUT && zucStatus->io.synch_di[zucAuxInputWaitIndex] == 0) ||
                (zucAuxInputWaitDIType == TIO_DIGITAL_INPUT && !(zucStatus->tio[0].tioStatus.DI_buffer & (1 << zucAuxInputWaitIndex))) ||
                (zucAuxInputWaitDIType == EXTIO_DIGITAL_INPUT && zucStatus->io.extio.extIOStat.extDI[zucAuxInputWaitIndex] == 0) ||
                (zucAuxInputWaitDIType == MB_SLAVE_DIGITAL_INPUT && zucStatus->io.synch_mb_slave_di[zucAuxInputWaitIndex] == 0) ||
                (zucAuxInputWaitDIType == PN_DIGITAL_INPUT && zucStatus->io.synch_pn_dev_di[zucAuxInputWaitIndex] == 0) ||
                (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && eip_di_low == 0))
            {
                pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitIndex = -1;
                pThreadInterpInfo->auxInputWaitMsg.waiting = false;
            }
            break;
        }
        case WAIT_MODE_FALL:
            if (zucStatus->io.synch_di[zucAuxInputWaitIndex] != 0)
            {
                pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitType = WAIT_MODE_LOW;
            }
            break;

        case WAIT_MODE_IMMEDIATE:
            pThreadInterpInfo->auxInputWaitMsg.zucAuxInputWaitIndex = -1;
            pThreadInterpInfo->auxInputWaitMsg.waiting = false;
            break;

        default:
            zucOperatorError(UNKNOWN_ERROR, "Unknown Wait Mode");
        }
    }
    return 0;
}

// executor function
static int zucTaskExecute(void)
{
    int retval = 0;
    int status;  // status of child from ZUC_SYSTEM_CMD
    pid_t pid;   // pid returned from waitpid()

    // first check for an abandoned system command and abort it
    if (zucSystzucmdPid != 0 && zucStatus->task.execState != ZUC_TASK_EXEC_WAITING_FOR_SYSTEM_CMD)
    {
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print("zucSystzucmd: abandoning process %d\n", zucSystzucmdPid);
        }
        kill(zucSystzucmdPid, SIGINT);
        zucSystzucmdPid = 0;
    }

    switch (zucStatus->task.execState)
    {
    case ZUC_TASK_EXEC_ERROR:

        /*! \todo FIXME-- duplicate code for abort,
		 also near end of main, when aborting on subordinate errors,
		 and in zucTaskIssueCommand() */

        // abort everything
        zucTaskAbort();
        mdi_execute_abort();

        // without zucTaskPlanClose(), a new run command resumes at
        // aborted line-- feature that may be considered later
        {
            int was_open = taskplanopen;
            zucTaskPlanClose();
            if (zuc_debug & ZUC_DEBUG_INTERP && was_open)
            {
                rcs_print("zucTaskPlanClose() called at %s:%d\n", __FILE__, __LINE__);
            }
        }

        // clear out pending command
        zucTaskCommand = 0;
        interp_list.clear();
        zucAbortCleanup(ZUC_ABORT_TASK_EXEC_ERROR);
        zucStatus->task.currentLine = 0;
        zucStatus->task.executingLine = 0;

        // clear out the interpreter state
        zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
        zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
        stepping = 0;
        steppingWait = 0;

        // now queue up command to resynch interpreter
        zucTaskQueueCommand(&taskPlanSynchCmd);

        retval = -1;
        break;

    case ZUC_TASK_EXEC_DONE:
        STEPPING_CHECK();
        if (!zucStatus->motion.traj.queueFull && zucStatus->task.interpState != ZUC_TASK_INTERP_PAUSED)
        {
            if (0 == zucTaskCommand)
            {
                // need a new command
                zucTaskCommand = interp_list.get();
                // rcs_print("main thread function: cmd=%p, rest size = %d\n", zucTaskCommand, interp_list.len());
                // interp_list now has line number associated with this-- get it
                if (0 != zucTaskCommand)
                {
                    zucTaskEager = 1;
                    zucStatus->task.sleepLine = 0;
                    zucStatus->task.currentLine = interp_list.get_line_number();
                    zucStatus->task.callLevel = zucTaskPlanLevel();
                    // and set it for all subsystems which use queued ids
                    zucTrajSetMotionId(zucStatus->task.currentLine);
                    if (zucStatus->motion.traj.queueFull)
                    {
                        zucStatus->task.execState = ZUC_TASK_EXEC_WAITING_FOR_MOTION_QUEUE;
                    }
                    else
                    {
                        zucStatus->task.execState = (enum ZUC_TASK_EXEC_ENUM)zucTaskCheckPreconditions(zucTaskCommand);
                        // if(zucTaskCommand->type==ZUC_TRAJ_LINEAR_MOVE_TYPE){
                        // 	rcs_print("taskState: %d, precond of Line cmd, lineNum: %d, current time is: %f\n", zucStatus->task.execState, zucStatus->task.currentLine, etime());
                        // } else if(zucTaskCommand->type==ZUC_MOTION_SET_DOUT_TYPE) {
                        // 	rcs_print("taskState: %d, precond of Dout cmd, tarStat: %d, current time is: %f\n", zucStatus->task.execState, ((ZUC_MOTION_SET_DOUT *)zucTaskCommand)->start, etime());
                        // }
                    }
                }
            }
            else
            {
                // have an outstanding command
                // if(zucTaskCommand->type==ZUC_MOTION_SET_DOUT_TYPE) {
                // 	rcs_print("Before issue Dout: taskState: %d, inpos = %d, queLen = %d, current time is: %f\n", zucStatus->task.execState, zucStatus->motion.traj.inpos, zucStatus->motion.traj.queue, etime());
                // }
                if (0 != zucTaskIssueCommand(zucTaskCommand))
                {
                    zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
                    retval = -1;
                }
                else
                {
                    zucStatus->task.execState = (enum ZUC_TASK_EXEC_ENUM)zucTaskCheckPostconditions(zucTaskCommand);
                    zucTaskEager = 1;
                }
                // if(zucTaskCommand->type==ZUC_TRAJ_LINEAR_MOVE_TYPE){
                // 	rcs_print("taskState: %d, postCond of Line cmd, lineNum: %d, current time is: %f\n", zucStatus->task.execState, zucStatus->task.currentLine, etime());
                // } else if(zucTaskCommand->type==ZUC_MOTION_SET_DOUT_TYPE) {
                // 	rcs_print("taskState: %d, postCond of Dout cmd, tarStat: %d, current time is: %f\n", zucStatus->task.execState, ((ZUC_MOTION_SET_DOUT *)zucTaskCommand)->start, etime());
                // }
                zucTaskCommand = 0;  // reset it
            }
        }
        break;

    case ZUC_TASK_EXEC_WAITING_FOR_MOTION_QUEUE:
        STEPPING_CHECK();
        if (!zucStatus->motion.traj.queueFull)
        {
            if (0 != zucTaskCommand)
            {
                zucStatus->task.execState = (enum ZUC_TASK_EXEC_ENUM)zucTaskCheckPreconditions(zucTaskCommand);
                zucTaskEager = 1;
            }
            else
            {
                zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
                zucTaskEager = 1;
            }
        }
        break;

    case ZUC_TASK_EXEC_WAITING_FOR_MOTION: {
        STEPPING_CHECK();
        zucMotionUpdate(&zucStatus->motion, zucStatus->ext_motion);
        if (zucStatus->motion.status == RCS_ERROR)
        {
            zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
        }
        else if (zucStatus->motion.status == RCS_DONE)
        {
            zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
            zucTaskEager = 1;
        }
        break;
    }

    case ZUC_TASK_EXEC_WAITING_FOR_IO:
        STEPPING_CHECK();
        zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
        zucTaskEager = 1;
        break;

    case ZUC_TASK_EXEC_WAITING_FOR_MOTION_AND_IO:
        STEPPING_CHECK();
        if (zucStatus->motion.status == RCS_ERROR)
        {
            zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
        }
        else if (zucStatus->motion.status == RCS_DONE)
        {
            zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
            zucTaskEager = 1;
        }
        break;

    case ZUC_TASK_EXEC_WAITING_FOR_SPINDLE_ORIENTED:
        STEPPING_CHECK();  // not sure
        break;

    case ZUC_TASK_EXEC_WAITING_FOR_DELAY:
        STEPPING_CHECK();
        // check if delay has passed
        zucStatus->task.delayLeft = taskExecDelayTimeout - etime();
        if (etime() >= taskExecDelayTimeout)
        {
            zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
            zucStatus->task.delayLeft = 0;
            if (zucStatus->task.input_timeout != 0)
            {
                zucStatus->task.input_timeout = 1;  // timeout occured
                zucTaskTimeoutUpdate();             // update to interp module
                                                    // we may need to pause
            }
            zucTaskEager = 1;
        }
        // delay can be also be because we wait for an input
        // if the index is set (not -1)
        if (zucAuxInputWaitIndex >= 0)
        {
            switch (zucAuxInputWaitType)
            {
            case WAIT_MODE_HIGH: {
                unsigned char eip_di_hi = 1;
                if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 1)
                {
                    int byteNum = zucAuxInputWaitIndex / 8;
                    zucAuxInputWaitIndex -= byteNum * 8;
                    eip_di_hi = 0x01 & (0xff & (zucStatus->io.synch_eip_in[byteNum] >> zucAuxInputWaitIndex));
                }
                else if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 0)
                {
                    eip_di_hi = zucStatus->io.synch_eip_adpt_di[zucAuxInputWaitIndex];
                }

                if ((zucAuxInputWaitDIType == DIGITAL_INPUT && zucStatus->io.synch_di[zucAuxInputWaitIndex] != 0) ||
                    (zucAuxInputWaitDIType == TIO_DIGITAL_INPUT && (zucStatus->tio[0].tioStatus.DI_buffer & (1 << zucAuxInputWaitIndex))) ||
                    (zucAuxInputWaitDIType == EXTIO_DIGITAL_INPUT && zucStatus->io.extio.extIOStat.extDI[zucAuxInputWaitIndex] != 0) ||
                    (zucAuxInputWaitDIType == MB_SLAVE_DIGITAL_INPUT && zucStatus->io.synch_mb_slave_di[zucAuxInputWaitIndex] != 0) ||
                    (zucAuxInputWaitDIType == PN_DIGITAL_INPUT && zucStatus->io.synch_pn_dev_di[zucAuxInputWaitIndex] != 0) ||
                    (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && eip_di_hi != 0))
                {
                    zucStatus->task.input_timeout = 0;  // clear timeout flag
                    zucTaskTimeoutUpdate();             // update to interp module
                    zucAuxInputWaitIndex = -1;
                    zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
                    zucStatus->task.delayLeft = 0;
                }
                break;
            }
            case WAIT_MODE_RISE:
                if (zucStatus->io.synch_di[zucAuxInputWaitIndex] == 0)
                {
                    zucAuxInputWaitType = WAIT_MODE_HIGH;
                }
                break;

            case WAIT_MODE_LOW: {
                unsigned char eip_di_low = 0;
                if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 1)
                {
                    int byteNum = zucAuxInputWaitIndex / 8;
                    zucAuxInputWaitIndex -= byteNum * 8;
                    eip_di_low = 0x01 & (0xff & (zucStatus->io.synch_eip_in[byteNum] >> zucAuxInputWaitIndex));
                }
                else if (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && zucStatus->io.eip_mode == 0)
                {
                    eip_di_low = zucStatus->io.synch_eip_adpt_di[zucAuxInputWaitIndex];
                }
                if ((zucAuxInputWaitDIType == DIGITAL_INPUT && zucStatus->io.synch_di[zucAuxInputWaitIndex] == 0) ||
                    (zucAuxInputWaitDIType == TIO_DIGITAL_INPUT && !(zucStatus->tio[0].tioStatus.DI_buffer & (1 << zucAuxInputWaitIndex))) ||
                    (zucAuxInputWaitDIType == EXTIO_DIGITAL_INPUT && zucStatus->io.extio.extIOStat.extDI[zucAuxInputWaitIndex] == 0) ||
                    (zucAuxInputWaitDIType == MB_SLAVE_DIGITAL_INPUT && zucStatus->io.synch_mb_slave_di[zucAuxInputWaitIndex] == 0) ||
                    (zucAuxInputWaitDIType == PN_DIGITAL_INPUT && zucStatus->io.synch_pn_dev_di[zucAuxInputWaitIndex] == 0) ||
                    (zucAuxInputWaitDIType == EIP_DIGITAL_INPUT && eip_di_low == 0))
                {
                    zucStatus->task.input_timeout = 0;  // clear timeout flag
                    zucTaskTimeoutUpdate();             // update to interp module
                    zucAuxInputWaitIndex = -1;
                    zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
                    zucStatus->task.delayLeft = 0;
                }
                break;
            }
            case WAIT_MODE_FALL:  //FIXME: implement different fall mode if needed
                if (zucStatus->io.synch_di[zucAuxInputWaitIndex] != 0)
                {
                    zucAuxInputWaitType = WAIT_MODE_LOW;
                }
                break;

            case WAIT_MODE_IMMEDIATE:
                zucStatus->task.input_timeout = 0;  // clear timeout flag
                zucTaskTimeoutUpdate();             // update to interp module
                zucAuxInputWaitIndex = -1;
                zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
                zucStatus->task.delayLeft = 0;
                break;

            default:
                zucOperatorError(UNKNOWN_ERROR, "Unknown Wait Mode");
            }
        }
        break;

    case ZUC_TASK_EXEC_WAITING_FOR_SYSTEM_CMD:
        STEPPING_CHECK();

        // if we got here without a system command pending, say we're done
        if (0 == zucSystzucmdPid)
        {
            zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
            break;
        }
        // check the status of the system command
        pid = waitpid(zucSystzucmdPid, &status, WNOHANG);

        if (0 == pid)
        {
            // child is still executing
            break;
        }

        if (-1 == pid)
        {
            // execution error
            if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
            {
                rcs_print("zucSystzucmd: error waiting for %d\n", zucSystzucmdPid);
            }
            zucSystzucmdPid = 0;
            zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
            break;
        }

        if (zucSystzucmdPid != pid)
        {
            // somehow some other child finished, which is a coding error
            if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
            {
                rcs_print("zucSystzucmd: error waiting for system command %d, we got %d\n", zucSystzucmdPid, pid);
            }
            zucSystzucmdPid = 0;
            zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
            break;
        }
        // else child has finished
        if (WIFEXITED(status))
        {
            if (0 == WEXITSTATUS(status))
            {
                // child exited normally
                zucSystzucmdPid = 0;
                zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
                zucTaskEager = 1;
            }
            else
            {
                // child exited with non-zero status
                if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
                {
                    rcs_print("zucSystzucmd: system command %d exited abnormally with value %d\n", zucSystzucmdPid, WEXITSTATUS(status));
                }
                zucSystzucmdPid = 0;
                zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
            }
        }
        else if (WIFSIGNALED(status))
        {
            // child exited with an uncaught signal
            if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
            {
                rcs_print("system command %d terminated with signal %d\n", zucSystzucmdPid, WTERMSIG(status));
            }
            zucSystzucmdPid = 0;
            zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
        }
        else if (WIFSTOPPED(status))
        {
            // child is currently being traced, so keep waiting
        }
        else
        {
            // some other status, we'll call this an error
            zucSystzucmdPid = 0;
            zucStatus->task.execState = ZUC_TASK_EXEC_ERROR;
        }
        break;

    default:
        // coding error
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print_error("invalid execState");
        }
        retval = -1;
        break;
    }
    return retval;
}

// called to allocate and init resources
#define RETRY_TIME 10.0     // seconds to wait for subsystems to come up
#define RETRY_INTERVAL 1.0  // seconds between wait tries for a subsystem
static int init_buffer()
{
    double end;
    int good;
    // get the NML command buffer
    if (!(zuc_debug & ZUC_DEBUG_NML))
    {
        set_rcs_print_destination(RCS_PRINT_TO_NULL);  // inhibit diag messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
        if (NULL != zucCommandBuffer)
        {
            delete zucCommandBuffer;
        }
        zucCommandBuffer = new RCS_CMD_CHANNEL(zucFormat, "zucCommand", "zuc", zuc_nmlfile);
        if (zucCommandBuffer->valid())
        {
            good = 1;
            break;
        }
        esleep(RETRY_INTERVAL);
        end -= RETRY_INTERVAL;
        if (done)
        {
            zuctask_shutdown();
            exit(1);
        }
    } while (end > 0.0);
    set_rcs_print_destination(RCS_PRINT_TO_STDOUT);  // restore diag messages
    if (!good)
    {
        rcs_print_error("can't get zucCommand buffer\n");
        return -1;
    }
    // get our command data structure
    zucCommand = zucCommandBuffer->get_address();

    // get the NML status buffer
    if (!(zuc_debug & ZUC_DEBUG_NML))
    {
        set_rcs_print_destination(RCS_PRINT_TO_NULL);  // inhibit diag messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
        if (NULL != zucStatusBuffer)
        {
            delete zucStatusBuffer;
        }
        zucStatusBuffer = new RCS_STAT_CHANNEL(zucFormat, "zucStatus", "zuc", zuc_nmlfile);
        if (zucStatusBuffer->valid())
        {
            good = 1;
            break;
        }
        esleep(RETRY_INTERVAL);
        end -= RETRY_INTERVAL;
        if (done)
        {
            zuctask_shutdown();
            exit(1);
        }
    } while (end > 0.0);
    set_rcs_print_destination(RCS_PRINT_TO_STDOUT);  // restore diag messages
    if (!good)
    {
        rcs_print_error("can't get zucStatus buffer\n");
        return -1;
    }

    if (!(zuc_debug & ZUC_DEBUG_NML))
    {
        set_rcs_print_destination(RCS_PRINT_TO_NULL);  // inhibit diag messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
        if (NULL != zucErrorBuffer)
        {
            delete zucErrorBuffer;
        }
        zucErrorBuffer = new NML(nmlErrorFormat, "zucError", "zuc", zuc_nmlfile);
        if (zucErrorBuffer->valid())
        {
            good = 1;
            break;
        }
        esleep(RETRY_INTERVAL);
        end -= RETRY_INTERVAL;
        if (done)
        {
            zuctask_shutdown();
            exit(1);
        }
    } while (end > 0.0);
    set_rcs_print_destination(RCS_PRINT_TO_STDOUT);  // restore diag messages
    if (!good)
    {
        rcs_print_error("can't get zucError buffer\n");
        return -1;
    }
    // get the timer
    if (!zucTaskNoDelay)
    {
        timer = new RCS_TIMER(zuc_task_cycle_time, "", "");
    }
    return 0;
}

static int zucInitMBSlaveConfig(const char* inifile)
{
    if (inifile == NULL)
        return -1;
    Util::IniParser ini_file(inifile);
    // ZucIniFile trajInifile;
    // trajInifile.Open(inifile);
    // trajInifile.EnableExceptions(ZucIniFile::ERR_CONVERSION);
    try
    {
        int mbSlaveCommType;
        ini_file.get("MODBUSSLAVE.COMMTYPE", mbSlaveCommType, 0);
        //trajInifile.Find(&mbSlaveCommType, "COMMTYPE", "MODBUSSLAVE");
        zucStatus->mbSlaveCommType = mbSlaveCommType;
        if (mbSlaveCommType == 0)
        {
            ini_file.get("MODBUSSLAVE.SLAVEID", zucStatus->mbSlaveCommInfo.rtuComm.slaveId, 1);
            ini_file.get("MODBUSSLAVE.BAUDRATE", zucStatus->mbSlaveCommInfo.rtuComm.baudrate, 9600);
            ini_file.get("MODBUSSLAVE.DATABIT", zucStatus->mbSlaveCommInfo.rtuComm.databits, 8);
            ini_file.get("MODBUSSLAVE.STOPBIT", zucStatus->mbSlaveCommInfo.rtuComm.stopbits, 1);
            ini_file.get("MODBUSSLAVE.PARITY", zucStatus->mbSlaveCommInfo.rtuComm.parity, 0);
            // trajInifile.Find(&zucStatus->mbSlaveCommInfo.rtuComm.slaveId, "SLAVEID", "MODBUSSLAVE");
            // trajInifile.Find(&zucStatus->mbSlaveCommInfo.rtuComm.baudrate, "BAUDRATE", "MODBUSSLAVE");
            // trajInifile.Find(&zucStatus->mbSlaveCommInfo.rtuComm.databits, "DATABIT", "MODBUSSLAVE");
            // trajInifile.Find(&zucStatus->mbSlaveCommInfo.rtuComm.stopbits, "STOPBIT", "MODBUSSLAVE");
            // trajInifile.Find(&zucStatus->mbSlaveCommInfo.rtuComm.parity, "PARITY", "MODBUSSLAVE");
        }
        else if (mbSlaveCommType == 1)
        {
            //const char* strIpAddr = trajInifile.Find("IPADDR", "MODBUSSLAVE");
            std::string s_ip;
            ini_file.get("MODBUSSLAVE.IPADDR", s_ip, "192.168.2.194");
            if (s_ip.c_str())
                strcpy(zucStatus->mbSlaveCommInfo.tcpComm.ipaddr, s_ip.c_str());
            ini_file.get("MODBUSSLAVE.PORT", zucStatus->mbSlaveCommInfo.tcpComm.port, 502);
            //trajInifile.Find(&zucStatus->mbSlaveCommInfo.tcpComm.port, "PORT", "MODBUSSLAVE");
        }
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}
/**
 * @brief 初始化总线
 * @note 必须确保各个bus都被开启，若发生错误，则创建仿真总线。否则后面的模块无法正常工作导致崩溃
 */
static int init_rtbus()
{
    //scb的总线
    auto scbinfo = hwdrv::HwMgr::get().get_scb_slot();
    std::shared_ptr<rtbus::RtCommuItf> bus;
    if (hwdrv::HwMgr::get().is_sim())
    {
        bus = rtbus::BusMgr::get()->create_bus("sim");
        rtbus::RtCommuItf::BusCfg cfg;
        bus->set_name("scbbus");
        bus->open(cfg);
    }
    else
    {
        bus = rtbus::BusMgr::get()->create_bus(scbinfo.bus.type);
        if (!bus.get())
        {
            bus = rtbus::BusMgr::get()->create_bus("sim");
            rtbus::RtCommuItf::BusCfg cfg;
            bus->set_name("scbbus");
            bus->open(cfg);
        }
        else
        {
            auto robbus = rtbus::BusMgr::parser_interface(scbinfo.bus.type, scbinfo.bus.interface);
            bus->set_name("scbbus");
            bus->open(robbus);
        }
    }
    rtbus::BusMgr::get()->append_bus(bus);

    //机器人本体的总线
    auto robot_slot_cnt = hwdrv::HwMgr::get().get_rob_slot_cnt();
    for (size_t i = 0; i < robot_slot_cnt; i++)
    {
        std::shared_ptr<rtbus::RtCommuItf> robbus;
        std::string busname = "robbus" + std::to_string(i);
        if (hwdrv::HwMgr::get().is_sim())
        {
            robbus = rtbus::BusMgr::get()->create_bus("sim");
            rtbus::RtCommuItf::BusCfg cfg;
            robbus->set_name(busname);
            robbus->open(cfg);
        }
        else
        {
            auto robinfo = hwdrv::HwMgr::get().get_rob_slot(i);
            robbus = rtbus::BusMgr::get()->create_bus(robinfo->bus.type);
            if (!robbus.get())
            {
                robbus = rtbus::BusMgr::get()->create_bus("sim");
                rtbus::RtCommuItf::BusCfg cfg;
                robbus->set_name(busname);
                robbus->open(cfg);
            }
            else
            {
                auto cfg = rtbus::BusMgr::parser_interface(robinfo->bus.type, robinfo->bus.interface);
                robbus->set_name(busname);
                robbus->open(cfg);
            }
        }
        rtbus::BusMgr::get()->append_bus(robbus);
    }
    //扩展轴的总线

    return 0;
}
static void init_task()
{
    task::RcStat::get()->init();
    //
}
int init_modules()
{
    init_rtbus();                               // 根据柜子的配置文件，加载总线
    rtdev::RtDevMgr::instance().init_cab_bp();  // 根据柜子类型，创建SCB对象，绑定总线，配置总线数据
    safety::SafeIOMgr::instance().init();       // 根据配置文件，创建安全IO

    int err = zucMotionInit();
    if (err)
    {
        rcs_print("zucMotionInit err=%d\n", err);
        return -1;
    }
    rcs_print("zucMotionInit good\n");

    err = funcio::init_funcio();
    if (err)
    {
        rcs_print("funcio init err=%d\n", err);
        return -1;
    }

    // _mbio.init();
    // zucInitMBSlaveConfig(zuc_inifile);
    // zucExtIOSetMode(1);
    init_task();
    return err;
}
static int zuctask_startup()
{
    int err = init_buffer();
    if (err)
    {
        rcs_print("init_buffer err=%d\n", err);
        return err;
    }
    init_modules();

    // if (zucMotionUpdate(&zucStatus->motion))
    // {
    //     rcs_print_error("can't read motion status\n");
    //     return -1;
    // }
    // zucMonitorUpdate(&zucStatus->monitor);

    // now the interpreter
    if (0 != zucTaskPlanInit())
    {
        rcs_print_error("can't initialize interpreter\n");
        return -1;
    }

    if (done)
    {
        zuctask_shutdown();
        exit(1);
    }

    // now task
    if (0 != zucTaskInit())
    {
        rcs_print_error("can't initialize task\n");
        return -1;
    }
    zucTaskUpdate(&zucStatus->task);

    if (zucStatus->cab_type == 1)  //v1.7.1后不支持cabv1
    {
        zucOperatorError(UNSUPPORTED_CABINET, "unsupport cabv1#{\"fixed_key\":[\"CabV1\"]}");
    }

    return 0;
}

// called to deallocate resources
static int zuctask_shutdown(void)
{
    // shut down the subsystems
    if (0 != zucStatus)
    {
        zucTaskHalt();
        zucTaskPlanExit();
    }
    // delete the timer
    if (0 != timer)
    {
        delete timer;
        timer = 0;
    }
    // delete the NML channels

    if (0 != zucErrorBuffer)
    {
        delete zucErrorBuffer;
        zucErrorBuffer = 0;
    }

    if (0 != zucStatusBuffer)
    {
        delete zucStatusBuffer;
        zucStatusBuffer = 0;
        zucStatus = 0;
    }

    if (0 != zucCommandBuffer)
    {
        delete zucCommandBuffer;
        zucCommandBuffer = 0;
        zucCommand = 0;
    }

    if (0 != zucStatus)
    {
        delete zucStatus;
        zucStatus = 0;
    }
    return 0;
}

static int iniLoad(const char* filename)
{
    Util::IniParser ini(filename);

    const char* inistring;
    char version[LINELEN], machine[LINELEN];
    double saveDouble;
    int saveInt;
    ini.get("ZUC.DEBUG", zuc_debug, 0);

    ini.get("TASK.INTERP_MAX_LEN", zuc_task_interp_max_len, 5, 0, 1000);
    ini.get("TASK.CYCLE_TIME", zuc_task_cycle_time, 0.0, 0, 1);
    if (zuc_task_cycle_time < 10e-6)
    {
        zucTaskNoDelay = 1;
    }

    ini.get("TASK.NO_FORCE_HOMING", no_force_homing, 0);
    ini.get("TASK.MDI_QUEUED_COMMANDS", max_mdi_queued_commands, MAX_MDI_QUEUE);
    ini.get("THREAD.RUNMODE_WHENPAUSE", threadRunModeWhenMainThreadPause, 0);
    rcs_print("subthread runModeWhenMainThreadPause = %d\n", threadRunModeWhenMainThreadPause);

    ini.get("CATEGORY_ONE_STOP.PROGRAMME_PAUSE", zucmotConfig.cat1StopEffect.program_pause, 1);

    return 0;
}

static void print_subThread_interp_error(ThreadInterpInfo* pThread, int retval)
{
    char interp_error_text_buf[LINELEN];
    char interp_stack_buf[LINELEN];
    MultiThreadInterp* pInterp = (MultiThreadInterp*)pThread->threadInterp.get();

    char dir[] = ZUC_SCRIPT_DIR;  //参考路径
    char filePath[PATH_MAX];
    strncpy(filePath,
            (dynamic_cast<Interp*>(pInterp))->_setup.filename,
            PATH_MAX);  // zucTaskAbort 会调用interp.close() ,如果有 脚本调用的话，会回退到主调脚本，丢失原本出错的脚本文件名。
    int errLine = (dynamic_cast<Interp*>(pInterp))->_setup.current_line;
    std::string md5 = get_md5sum(std::string(filePath));

    zucTaskAbort();
    int index = 0;
    int suberrcode = 0;
    if (retval == 0)
    {
        return;
    }

    if (0 != zucStatus)
    {
        zucStatus->task.interpreter_errcode = retval;
    }

    interp_error_text_buf[0] = 0;
    pInterp->error_text(retval, &suberrcode, interp_error_text_buf, LINELEN);
    if (0 != interp_error_text_buf[0])
    {
        rcs_print_error("interp_error: %s\n", interp_error_text_buf);
    }

    int errCode = 0;
    if (retval == INTERP_ERROR)
    {
        errCode = suberrcode;
    }
    else
    {
        errCode = PROGRAM_FILE_NOT_OPEN;
    }

    if (strncmp(filePath, dir, strlen(dir)) == 0)
    {
        strncpy(filePath, &filePath[strlen(dir)], PATH_MAX - strlen(dir));  // 纪录相对于 ZUC_SCRIPT_DIR"/" 的文件路径。
    }

    if (strlen(filePath) <= 0 || md5.compare("0") == 0)
    {
        //  文件名异常， 不追加json字段
        zucOperatorError(errCode, "%s", interp_error_text_buf);
    }
    else
    {
        char seps[] = "#";
        char* token = NULL;
        token = strtok(interp_error_text_buf, seps);
        token = strtok(NULL, seps);
        if (token == NULL)
        {
            token = (char*)"";
        }
        //{"fixed_key":["0x123","0x122","0x321","0x321"],"param":{"0":"1"},"others":{"md5":"dafwev2113"}}
        std::string jsonStr = "{\"fixed_key\":[\"" + std::string(filePath) + "\",\"" + std::to_string(errLine) + "\"]," + std::string(token) +
                              "\"others\":{\"md5\":\"" + md5 + "\"}}";
        // std::string jsonStr = make_json(str);
        zucOperatorError(errCode, "[Thread %s]: %s #%s", pThread->thread_alias.c_str(), interp_error_text_buf, jsonStr.c_str());
    }

    index = 0;
    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        // rcs_print("Interpreter stack: \t");
        while (index < 5)
        {
            interp_stack_buf[0] = 0;
            pInterp->stack_name(index, interp_stack_buf, LINELEN);
            if (0 == interp_stack_buf[0])
            {
                break;
            }
            rcs_print(" - %s ", interp_stack_buf);
            index++;
        }
        // rcs_print("\n");
    }
}

void* procSubThread(void* arg)
{
    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);       //允许退出线程
    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);  //设置立即取消
    ThreadInterpInfo* pThreadInterpInfo = (ThreadInterpInfo*)(arg);
    pThreadInterpInfo->threadInterp->_waitFlag = 0;
    bool subThreadDelayFlag = false;
    double subThreadDelayTimeout = 0.0;

    // rcs_print("#####thread %d: started\n", pThreadInterpInfo->thread_id);
    pThreadInterpInfo->auxInputWaitMsg.waiting = false;
    pThreadInterpInfo->threadInterp->set_thread_id(pThreadInterpInfo->thread_id);
    if (pThreadInterpInfo)
    {
        pThreadInterpInfo->nmlcmd_list->clear();
        while (1)
        {
            try
            {
                if (thread_exit_flag)
                {  //程序退出或destroyThread
                    break;
                }

                if (pThreadInterpInfo->destroied_flag == 1)
                {
                    usleep(10000);
                    continue;
                }

                if (pThreadInterpInfo->destroy_flag)
                {
                    for (int j = 0; j < MAX_SUB_THREAD; ++j)
                    {
                        if (strcmp(&(zucStatus->sub_thread_state[j][0]), pThreadInterpInfo->thread_alias.c_str()) == 0)
                        {
                            zucStatus->sub_thread_state[j][0] = '\0';
                            break;
                        }
                    }
                    pThreadInterpInfo->destroy_flag = 0;
                    threadInterpNum--;
                    pThreadInterpInfo->destroied_flag = 1;
                    pThreadInterpInfo->join_flag = 0;
                    pThreadInterpInfo->threadInterp->close();
                    pThreadInterpInfo->nmlcmd_list->clear();
                    pThreadInterpInfo->auxInputWaitMsg.waiting = false;
                    pThreadInterpInfo->thread_alias = "";
                    // printf("#######################thread[%d] destroied.  threadInterpNum = %d\n",pThreadInterpInfo->thread_id-1, threadInterpNum);
                    continue;
                }

                NMLmsg* threadMsg = NULL;

                // 当threadRunModeWhenMainThreadPause非0时表示即时主程序暂停，子线程也不暂停,注意是暂停
                // 当主程序停止时，子线程还是正常停止
                if (thread_pause_flag && threadRunModeWhenMainThreadPause)
                    thread_pause_flag = false;

                if (subThreadDelayFlag)
                {
                    if (etime() < subThreadDelayTimeout)
                    {
                        usleep(1000);
                        continue;
                    }
                    else
                    {
                        subThreadDelayFlag = false;
                    }
                }

                if (!thread_pause_flag && !(pThreadInterpInfo->auxInputWaitMsg.waiting))
                {
                    while (1)
                    {
                        if (!pthread_mutex_trylock(&cmd_chn_lock))
                        {
                            break;
                        }
                        usleep(1000);
                    }
                    int res;
                    res = pThreadInterpInfo->threadInterp->read(((MultiThreadInterp*)pThreadInterpInfo->threadInterp.get())->_setup.parameters,
                                                                pThreadInterpInfo->thread_id);
                    if (res == INTERP_ENDFILE || thread_exit_flag)
                    {  //脚本执行完成
                        pThreadInterpInfo->nmlcmd_list->clear();
                        pthread_mutex_unlock(&cmd_chn_lock);
                        pThreadInterpInfo->destroy_flag = 1;
                        continue;
                        // break;
                    }
                    if (res > INTERP_MIN_ERROR)
                    {
                        print_subThread_interp_error(pThreadInterpInfo, res);
                        pthread_mutex_unlock(&cmd_chn_lock);
                        pThreadInterpInfo->destroy_flag = 1;
                        zucOperatorDisplay(INTERP_EXECUTION_ERROR, "Program execution error.");
                        zucTaskAbort();
                        continue;
                    }

                    res = pThreadInterpInfo->threadInterp->execute();
                    if (res > INTERP_MIN_ERROR)
                    {
                        print_subThread_interp_error(pThreadInterpInfo, res);
                        pthread_mutex_unlock(&cmd_chn_lock);
                        pThreadInterpInfo->destroy_flag = 1;
                        zucTaskAbort();
                        continue;
                    }
                    if (pThreadInterpInfo->nmlcmd_list->len())
                        threadMsg = pThreadInterpInfo->nmlcmd_list->get();
                    pthread_mutex_unlock(&cmd_chn_lock);
                }

                if (pThreadInterpInfo->auxInputWaitMsg.waiting)
                {
                    subThreadWaitInputExecute(pThreadInterpInfo);
                }
                if (threadMsg)
                {
                    // rcs_print("#####thread %d: message obtained, nmltype = %d\n", pThreadInterpInfo->thread_id, threadMsg->type);
                    if (ZUC_TRAJ_DELAY* delayMsg = dynamic_cast<ZUC_TRAJ_DELAY*>(threadMsg))
                    {
                        // rcs_print("#####thread %d: function delaying, time = %f\n", pThreadInterpInfo->thread_id, delayMsg->delay);
                        if (!thread_pause_flag)
                        {
                            // rcs_print("#####thread %d: function delaying, time = %f\n", pThreadInterpInfo->thread_id, delayMsg->delay);
                            // usleep(delayMsg->delay*1000000);
                            subThreadDelayTimeout = etime() + delayMsg->delay;
                            subThreadDelayFlag = true;
                            // rcs_print("#####thread %d: function delaying complete\n", pThreadInterpInfo->thread_id);
                        }
                    }
                    else if (ZUC_AUX_INPUT_WAIT* inputWaitMsg = dynamic_cast<ZUC_AUX_INPUT_WAIT*>(threadMsg))
                    {
                        if (!thread_pause_flag)
                        {
                            subThreadAuxInputWaitMsg(inputWaitMsg, pThreadInterpInfo);
                        }
                    }
                    else
                    {
                        while (1)
                        {
                            if (!pthread_mutex_trylock(&cmd_chn_lock))
                                break;
                            usleep(1000);
                        }

                        while (thread_pause_flag) { pthread_cond_wait(&thread_cond, &cmd_chn_lock); }

                        // rcs_print("#####thread %d: function dout\n", pThreadInterpInfo->thread_id);
                        zucCommand = dynamic_cast<RCS_CMD_MSG*>(threadMsg);
                        zucCommand->serial_number = zucStatus->echo_serial_number + 1;
                        // rcs_print("#####thread %d: function command type = %d\n", pThreadInterpInfo->thread_id, zucCommand->type);
                        zucTaskPlan();
                        // rcs_print("#####thread %d: zucTaskPlan complete\n", pThreadInterpInfo->thread_id);
                        // do task
                        zucStatus->task.command_type = zucCommand->type;
                        zucStatus->task.echo_serial_number = zucCommand->serial_number;
                        // do top level
                        zucStatus->command_type = zucCommand->type;
                        zucStatus->echo_serial_number = zucCommand->serial_number;
                        zucCommand = NULL;
                        zucTaskExecute();
                        pthread_mutex_unlock(&cmd_chn_lock);
                    }
                }
                usleep(1000);
            }
            catch (...)
            {
                rcs_print("#####thread %d exception\n", pThreadInterpInfo->thread_id);
                throw;
            }
        }

        usleep(1000);
        for (auto i = threadStatusList.begin(); i != threadStatusList.end(); ++i)
        {
            auto it = (*i).begin();

            if (it->first == pThreadInterpInfo->pth_t)
            {
                threadStatusList.erase(*i);
                break;
            }
        }
    }
    // rcs_print("#####thread %s end\n", pThreadInterpInfo->thread_alias.c_str());
    pThreadInterpInfo->threadInterp->close();
    pThreadInterpInfo->nmlcmd_list->clear();
    pThreadInterpInfo->thread_alias = "";
    pThreadInterpInfo->auxInputWaitMsg.waiting = false;
    pThreadInterpInfo->is_running = 0;
    pThreadInterpInfo->join_flag = 0;
    threadInterpNum--;
    return (void*)(NULL);
}

int procInstThread()
{
    int ret = zucCommandBuffer->read();
    /* if (ret == 0)
    {
        // for (int i = 0; i < MAX_SUB_THREAD; ++i)
        // {
        //     if (threadInterpList[i].join_flag)
        //     {
        //         if (!threadInterpList[i].is_running || threadInterpList[i].destroied_flag)
        //             threadInterpList[i].join_flag = 0;
        //         return 1;
        //     }
        // }
    }
    else  */
    if (0 != ret)
    {
        zucCommand = zucCommandBuffer->get_address();
        zucTaskPlan();

        // do task
        zucStatus->task.command_type = zucCommand->type;
        zucStatus->task.echo_serial_number = zucCommand->serial_number;
        // do top level
        zucStatus->command_type = zucCommand->type;
        zucStatus->echo_serial_number = zucCommand->serial_number;
        // zucCommand = NULL;
    }

    // if(threadInterpList[0].pause_flag == 1) {
    // 	zucTrajPause();
    // 	if (zucStatus->task.interpState != ZUC_TASK_INTERP_PAUSED) {
    // 		interpResumeState = zucStatus->task.interpState;
    // 	}
    // 	zucStatus->task.interpState = ZUC_TASK_INTERP_PAUSED;
    // 	zucStatus->task.task_paused = 1;
    // 	thread_pause_flag = true;
    // 	threadInterpList[0].pause_flag = 0;
    // }

    return 0;
}

/*
 syntax: a.out {-d -ini <inifile>} {-nml <nmlfile>} {-shm <key>}
 */
int zuctaskmain(int argc, char* argv[])
{
    int taskPlanError = 0;
    int taskExecuteError = 0;

    pthread_mutexattr_init(&cmd_chn_lock_attr);
    pthread_mutexattr_setpshared(&cmd_chn_lock_attr, PTHREAD_PROCESS_PRIVATE);
    pthread_mutex_init(&cmd_chn_lock, &cmd_chn_lock_attr);

    // loop until done
    done = 0;

    // 加载部分zuc字段，部分task字段，部分thread字段
    // get configuration information
    iniLoad(ZUC_USER_CONFIG_FILE(usersettings.ini));

    if (done)
    {
        zuctask_shutdown();
        exit(1);
    }

    // get our status data structure
    zucStatus = new ZUC_STAT;
    // reflect the initial value of ZUC_DEBUG in zucStatus->debug
    zucStatus->debug = zuc_debug;
    zucStatus->cab_type = zuc_cab_type;

    // 默认机器人为6轴
    zucStatus->motion.robot_joint_num = 6;

    // initialize everything
    if (0 != zuctask_startup())
    {
        zuctask_shutdown();
        exit(1);
    }

    // robot_motion_task->zucTrajSetMode(ZUC_TRAJ_MODE_FREE);

    // startTime = etime();	// set start time before entering loop;
    // // first_start_time = startTime;
    // endTime = startTime;
    // // it will be set at end of loop from now on
    // minTime = DBL_MAX;		// set to value that can never be exceeded
    // maxTime = 0.0;		// set to value that can never be underset
    zucMotionUpdate(&zucStatus->motion, zucStatus->ext_motion);
    if (0 != usrmotReadZucmotConfig(&zucmotConfig))
    {
        rcs_print("%s failed usrmotReadZucmotconfig()\n", __FILE__);
    }
    zucTaskInitFinish();
    while (!done)
    {
        try
        {
            static int gave_soft_limit_message = 0;
            // check_ini_hal_items(zucStatus->motion.traj.joints);
            if (multi_thread_flag)
            {
                if (threadStatusList.size() < MAX_SUB_THREAD + 1)
                {
                    // check sub thread
                    for (int i = 0; i < MAX_SUB_THREAD; i++)
                    {
                        if (threadInterpList[i].new_flag && threadInterpList[i].join_flag != true)
                        {
                            threadInterpList[i].new_flag = 0;
                            if (threadInterpList[i].is_running == false)
                            {
                                pthread_t gThreadId;
                                pthread_attr_t attr;
                                pthread_attr_init(&attr);
                                pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
                                // pthread_attr_setinheritsched (&attr, PTHREAD_EXPLICIT_SCHED);
                                pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
                                // PTHREAD_ATTR_SETSCHEDPOLICY (&attr, SCHED_PT2POSIX);

                                /* initialize thread's priority value */
                                struct sched_param param;
                                param.sched_priority = 55;
                                pthread_attr_setschedparam(&attr, &param);

                                int err = pthread_create(&gThreadId, &attr, procSubThread, &threadInterpList[i]);
                                if (err)
                                    rtapi_print("--------------------pthread_create status:%d\n", err);
                                threadInterpList[i].is_running = 1;
                                threadInterpList[i].pth_t = gThreadId;
                                threadStatusList.insert(std::map<pthread_t, std::string>({{threadInterpList[i].pth_t, threadInterpList[i].thread_alias}}));
                            }
                            rcs_print("thread created>>>>>>>>>>: index = %d, threadid = %d\n", i, threadInterpList[i].thread_id);
                            for (int j = 0; j < MAX_SUB_THREAD; ++j)
                            {
                                if (zucStatus->sub_thread_state[j][0] == '\0')
                                {
                                    strcpy(&(zucStatus->sub_thread_state[j][0]), threadInterpList[i].thread_alias.c_str());
                                    break;
                                }
                            }
                        }
                    }
                }
                else
                {
                    interp_list.clear();
                    zucTaskAbort();
                    zucAbortCleanup(ZUC_ABORT_THREAD_NUM_OVER_LIMIT, "interpreter error:thread number over limit");
                    rcs_print("interpreter error:thread number over limit");
                }
            }
            else
            {
                // read command
                if (0 != zucCommandBuffer->read())
                {
                    taskPlanError = 0;
                    taskExecuteError = 0;
                }
            }

            if (multi_thread_flag)
            {
                pthread_mutex_lock(&cmd_chn_lock);
                if (!procInstThread())
                {
                    if (0 != zucTaskPlan())
                    {
                        taskPlanError = 1;
                    }

                    if (0 != zucTaskExecute())
                    {
                        taskExecuteError = 1;
                    }
                }
                pthread_mutex_unlock(&cmd_chn_lock);
            }
            else
            {
                // run control cycle
                if (0 != zucTaskPlan())
                {
                    taskPlanError = 1;
                }

                if (0 != zucTaskExecute())
                {
                    taskExecuteError = 1;
                }
            }

            // update subordinate status
            zucStatus->isDiagCollecting = zucDiagIsCollectingData();
            zucMotionUpdate(&zucStatus->motion, zucStatus->ext_motion);
            zucMonitorUpdate(&zucStatus->monitor);
            // synchronize subordinate states
            if (!zucStatus->motion.powered_on)
            {
                if (zucStatus->motion.category_one_stop_program_pause == 1)
                {
                    if (zucStatus->motion.is_enabled)
                    {
                        // zucTrajDisable();
                    }
                }
                else
                {
                    if (zucStatus->motion.is_enabled)
                    {
                        // zucTrajDisable();
                        // zucTaskAbort();
                        // zucJointUnhome(-2);  // only those joints which are volatile_home
                        // mdi_execute_abort();
                        // zucAbortCleanup(ZUC_ABORT_AUX_ESTOP);
                        // zucTaskPlanSynch();
                    }
                }
            }

            // synchroize extIO status
            checkExtioStat();

            checkSensorMgrStat();

            if (!zucStatus->motion.on_soft_limit)
            {
                gave_soft_limit_message = 0;
            }

            // check for subordinate errors, and halt task if so
            if (zucStatus->motion.status == RCS_ERROR && zucStatus->motion.on_soft_limit)
            {
                if (!gave_soft_limit_message)
                {
                    gave_soft_limit_message = 1;
                }
                zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
            }
            else if (zucStatus->motion.status == RCS_ERROR)
            {
                // zucTaskAbort(); #TODO LW
                mdi_execute_abort();
                // without zucTaskPlanClose(), a new run command resumes at
                // aborted line-- feature that may be considered later
                {
                    int was_open = taskplanopen;
                    zucTaskPlanClose();
                    if (zuc_debug & ZUC_DEBUG_INTERP && was_open)
                    {
                        rcs_print("zucTaskPlanClose() called at %s:%d\n", __FILE__, __LINE__);
                    }
                }

                // clear out the pending command
                zucTaskCommand = 0;
                interp_list.clear();
                zucStatus->task.currentLine = 0;
                zucStatus->task.executingLine = 0;

                zucAbortCleanup(ZUC_ABORT_MOTION_OR_IO_RCS_ERROR);

                // clear out the interpreter state
                zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
                // printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>%s[%d]  ZucMotEnable()  interpState = %d\n", __FILE__, __LINE__, zucStatus->task.interpState);
                zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
                stepping = 0;
                steppingWait = 0;
                // zucTaskSetMode(ZUC_TASK_MODE_MANUAL);  #TODO LW
                // now queue up command to resynch interpreter
                zucTaskQueueCommand(&taskPlanSynchCmd);
            }

            // update task-specific status
            zucUpdate(zucStatus);
            zucTaskUpdate(&zucStatus->task);

            if (zucStatus->task.task_paused && stepping &&
                (zucStatus->task.delayLeft > 0.001 || (zucStatus->motion.steppingWait && !zucStatus->motion.traj.inpos)))
            {
                zucStatus->steppingWait = 1;
            }
            else
            {
                zucStatus->steppingWait = 0;
            }
            // 进入退出调试模式时更新状态
            if (zucStatus->isSteppingMode != stepping)
            {
                rtapi_print("zucStatus->isSteppingMode: %d , ---> stepping: %d \n", zucStatus->isSteppingMode, stepping);
                zucStatus->isSteppingMode = stepping;
                zucStatus->steppingState = ZUC_TASK_INTERP_IDLE;
            }
            if (zucStatus->motion.traj.inpos == FALSE && zucStatus->steppingWait)
            {
                if (zucStatus->steppingState != ZUC_TASK_INTERP_PAUSED)
                    zucStatus->steppingState = ZUC_TASK_INTERP_READING;
            }
            if (zucStatus->motion.traj.inpos == true || (zucStatus->steppingWait == 0 && zucStatus->steppingState != ZUC_TASK_INTERP_PAUSED))
            {
                zucStatus->steppingState = ZUC_TASK_INTERP_IDLE;
            }

            {
                // restore the tool and user id
                if (zucStatus->task.progInExec > 0 && zucStatus->motion.traj.inpos && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
                {
                    ++zucStatus->task.progInExec;
                    if (zucStatus->task.progInExec > 5)
                    {
                        zucStatus->task.progInExec = 0;
                    }
                }

                // check safe zone settings
                static int lastProgInExec = 0;
                if ((lastProgInExec == 0 && zucStatus->task.progInExec) || (lastProgInExec && zucStatus->task.progInExec == 0))
                {
                    zucUpdateSafeZoneEna();  // 程序执行或退出的时候更新安全平面使能设置
                }
                lastProgInExec = zucStatus->task.progInExec;
            }

            if (!multi_thread_flag)
            {
                // do task
                zucStatus->task.command_type = zucCommand->type;
                zucStatus->task.echo_serial_number = zucCommand->serial_number;

                // do top level
                zucStatus->command_type = zucCommand->type;
                zucStatus->echo_serial_number = zucCommand->serial_number;
            }

            if (taskPlanError || taskExecuteError || zucStatus->task.execState == ZUC_TASK_EXEC_ERROR || zucStatus->motion.status == RCS_ERROR)
            {
                zucStatus->status = RCS_ERROR;
                zucStatus->task.status = RCS_ERROR;
            }
            else if (!taskPlanError && !taskExecuteError && zucStatus->task.execState == ZUC_TASK_EXEC_DONE && zucStatus->motion.status == RCS_DONE &&
                     mdi_execute_queue.len() == 0 && interp_list.len() == 0 && zucTaskCommand == 0 && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
            {
                zucStatus->status = RCS_DONE;
                zucStatus->task.status = RCS_DONE;
            }
            else
            {
                zucStatus->status = RCS_EXEC;
                zucStatus->task.status = RCS_EXEC;
            }

            // since zucStatus was passed to the WM init functions, it
            // will be updated in the _update() functions above. There's
            // no need to call the individual functions on all WM items.
            zucStatusBuffer->write(zucStatus);

            //printf(" [ task main  ] :  zucStatus->task.interpState = %d \n", zucStatus->task.interpState);
            // wait on timer cycle, if specified, or calculate actual
            // interval if ini file says to run full out via
            // endTime = etime();
            // deltaTime = endTime - startTime;
            // if (deltaTime < minTime)
            // 	minTime = deltaTime;
            // else if (deltaTime > maxTime)
            // 	maxTime = deltaTime;
            // startTime = endTime;
            // if (deltaTime > (latency_excursion_factor * zuc_task_cycle_time)) {
            // 	num_latency_warnings++;
            // }

            // updateLicense();		// this will affect the task cycle

            if ((zucTaskNoDelay) || (zucTaskEager))
            {
                usleep(500);
                zucTaskEager = 0;
            }
            else
            {
                timer->wait();
            }
        }
        catch (...)
        {
        }

    }  // end of while (! done)

    // rcs_print("task: %u cycles, min=%.6f, max=%.6f, avg=%.6f, %u latency excursions (> %dx expected cycle time of %.6fs)\n",
    // 		zucStatus->task.heartbeat, minTime, maxTime,
    // 		(zucStatus->task.heartbeat != 0) ? (endTime - first_start_time) / zucStatus->task.heartbeat : -1.0, num_latency_warnings,
    // 		latency_excursion_factor, zuc_task_cycle_time);

    zuctask_shutdown();
    exit(0);
}
